void createTerrain(ChSystem & sys, double dia, double W, double H, double L)
{
int x = W/dia-3;
int y = H/dia;
int z = L/dia-3;
double mass = 0.05 * 4/3*CH_C_PI*pow(particleDiameter/2.0,3.0);// density * volume
double inertia = 0.4*mass*particleDiameter*particleDiameter/4.0;
ChSharedBodyPtr particle;
for (int i=3; i<x; i++)
{
for (int j=0; j<y; j++)
{
for (int k=3; k<z; k++)
{
particle=ChSharedBodyPtr(new ChBody);
particle->SetPos(ChVector<>(-W/2.0,-boxDrop,-(chassisL/2.0)-2.0) + ChVector<>(i+ChRandom()-0.5,j,k+ChRandom()-0.5) * dia);
particle->SetMass(mass);
particle->SetInertiaXX(ChVector<>(inertia,inertia,inertia));
particle->GetCollisionModel()->AddSphere(dia/2.0);
particle->SetCollide(true);
particle->SetPos_dt(ChVector<>(ChRandom()-0.5,-ChRandom(),ChRandom()-0.5)*10 );
sys.AddBody(particle);
}
}
}
}
// Delete the tank object, deleting also all bodies corresponding to
// the various parts and removing them from the physical system. Also
// removes constraints from the system.
~MySimpleTank()
{
ChSystem* mysystem = truss->GetBody()->GetSystem(); // trick to get the system here
// When a ChBodySceneNode is removed via ->remove() from Irrlicht 3D scene manager,
// it is also automatically removed from the ChSystem (the ChSystem::RemoveBody() is
// automatically called at Irrlicht node deletion - see ChBodySceneNode.h ).
// For links, just remove them from the ChSystem using ChSystem::RemoveLink()
mysystem->RemoveLink(link_revoluteRF);
mysystem->RemoveLink(link_revoluteLF);
mysystem->RemoveLink(link_revoluteRB);
mysystem->RemoveLink(link_revoluteLB);
truss->remove();
wheelRF->remove();
wheelLF->remove();
wheelRB->remove();
wheelLB->remove();
}
void ChEmitterAsset::Update(ChPhysicsItem* updater, const ChCoordsys<>& coords) {
ChSystem* msys = updater->GetSystem();
if (!msys)
return;
double mt = msys->GetChTime();
if (mt < this->last_t)
last_t = mt;
double dt = mt - last_t;
last_t = mt;
if (dt == 0)
return;
//***TODO*** a better way to deduce dt, ex. by using flags in Update(),
// to know if Update really follows a t+dt update, etc.
// Create the particles!
ChFrameMoving<> mframe(coords);
// special case: the owner is a body? so use also speed info
if(ChBody* mbody = dynamic_cast<ChBody*>(updater)) {
ChFrameMoving<> bodyframe = mbody->GetFrame_REF_to_abs();
ChFrameMoving<> relcoords;
bodyframe.TransformParentToLocal(mframe, relcoords);
relcoords.SetCoord_dt(CSYSNULL);
relcoords.SetCoord_dtdt(CSYSNULL);
bodyframe.TransformLocalToParent(relcoords, mframe);
}
this->memitter.EmitParticles(*msys, dt, mframe);
}
// Delete the car object, deleting also all bodies corresponding to
// the various parts and removing them from the physical system. Also
// removes constraints from the system.
~MySimpleForklift()
{
ChSystem* mysystem = truss->GetBody()->GetSystem(); // trick to get the system here
// When a ChBodySceneNode is removed via ->remove() from Irrlicht 3D scene manager,
// it is also automatically removed from the ChSystem (the ChSystem::RemoveBody() is
// automatically called at Irrlicht node deletion - see ChBodySceneNode.h ).
// For links, just remove them from the ChSystem using ChSystem::RemoveLink()
mysystem->RemoveLink(link_revoluteRF);
mysystem->RemoveLink(link_revoluteLF);
mysystem->RemoveLink(link_steer_engineB);
mysystem->RemoveLink(link_engineB);
mysystem->RemoveLink(link_engineArm);
mysystem->RemoveLink(link_prismaticFork);
mysystem->RemoveLink(link_actuatorFork);
truss->remove();
wheelRF->remove();
wheelLF->remove();
wheelB->remove();
spindleB->remove();
arm->remove();
fork->remove();
}
int main(int argc, char* argv[])
{
ChSystem system;
// Disable gravity
system.Set_G_acc(ChVector<>(0, 0, 0));
// Set the half-length of the two shafts
double hl = 2;
// Set the bend angle between the two shafts (positive rotation about the
// global X axis)
double angle = CH_C_PI / 6;
double cosa = std::cos(angle);
double sina = std::sin(angle);
ChQuaternion<> rot = Q_from_AngX(angle);
// Create the ground (fixed) body
// ------------------------------
ChSharedPtr<ChBody> ground(new ChBody);
system.AddBody(ground);
ground->SetIdentifier(-1);
ground->SetBodyFixed(true);
ground->SetCollide(false);
// attach visualization assets to represent the revolute and cylindrical
// joints that connect the two shafts to ground
{
ChSharedPtr<ChCylinderShape> cyl_1(new ChCylinderShape);
cyl_1->GetCylinderGeometry().p1 = ChVector<>(0, 0, -hl - 0.2);
cyl_1->GetCylinderGeometry().p2 = ChVector<>(0, 0, -hl + 0.2);
cyl_1->GetCylinderGeometry().rad = 0.3;
ground->AddAsset(cyl_1);
ChSharedPtr<ChCylinderShape> cyl_2(new ChCylinderShape);
cyl_2->GetCylinderGeometry().p1 = ChVector<>(0, -(hl - 0.2) * sina, (hl - 0.2) * cosa);
cyl_2->GetCylinderGeometry().p2 = ChVector<>(0, -(hl + 0.2) * sina, (hl + 0.2) * cosa);
cyl_2->GetCylinderGeometry().rad = 0.3;
ground->AddAsset(cyl_2);
}
// Create the first shaft body
// ---------------------------
ChSharedPtr<ChBody> shaft_1(new ChBody);
system.AddBody(shaft_1);
shaft_1->SetIdentifier(1);
shaft_1->SetBodyFixed(false);
shaft_1->SetCollide(false);
shaft_1->SetMass(1);
shaft_1->SetInertiaXX(ChVector<>(1, 1, 0.2));
shaft_1->SetPos(ChVector<>(0, 0, -hl));
shaft_1->SetRot(ChQuaternion<>(1, 0, 0, 0));
// Add visualization assets to represent the shaft (a box) and the arm of the
// universal joint's cross associated with this shaft (a cylinder)
{
ChSharedPtr<ChBoxShape> box_1(new ChBoxShape);
box_1->GetBoxGeometry().Size = ChVector<>(0.15, 0.15, 0.9 * hl);
shaft_1->AddAsset(box_1);
ChSharedPtr<ChCylinderShape> cyl_2(new ChCylinderShape);
cyl_2->GetCylinderGeometry().p1 = ChVector<>(-0.2, 0, hl);
cyl_2->GetCylinderGeometry().p2 = ChVector<>(0.2, 0, hl);
cyl_2->GetCylinderGeometry().rad = 0.05;
shaft_1->AddAsset(cyl_2);
ChSharedPtr<ChColorAsset> col(new ChColorAsset);
col->SetColor(ChColor(0.6f, 0, 0));
shaft_1->AddAsset(col);
}
// Create the second shaft body
// ----------------------------
// The second shaft is identical to the first one, but initialized at an angle
// equal to the specified bend angle.
ChSharedPtr<ChBody> shaft_2(new ChBody);
system.AddBody(shaft_2);
shaft_2->SetIdentifier(1);
shaft_2->SetBodyFixed(false);
shaft_2->SetCollide(false);
shaft_2->SetMass(1);
shaft_2->SetInertiaXX(ChVector<>(1, 1, 0.2));
shaft_2->SetPos(ChVector<>(0, -hl * sina, hl * cosa));
shaft_2->SetRot(rot);
// Add visualization assets to represent the shaft (a box) and the arm of the
// universal joint's cross associated with this shaft (a cylinder)
{
ChSharedPtr<ChBoxShape> box_1(new ChBoxShape);
box_1->GetBoxGeometry().Size = ChVector<>(0.15, 0.15, 0.9 * hl);
shaft_2->AddAsset(box_1);
ChSharedPtr<ChCylinderShape> cyl_2(new ChCylinderShape);
cyl_2->GetCylinderGeometry().p1 = ChVector<>(0, -0.2, -hl);
cyl_2->GetCylinderGeometry().p2 = ChVector<>(0, 0.2, -hl);
cyl_2->GetCylinderGeometry().rad = 0.05;
shaft_2->AddAsset(cyl_2);
ChSharedPtr<ChColorAsset> col(new ChColorAsset);
col->SetColor(ChColor(0, 0, 0.6f));
shaft_2->AddAsset(col);
}
// Connect the first shaft to ground
// ---------------------------------
// Use a ChLinkEngine to impose both the revolute joint constraints, as well
// as constant angular velocity. The joint is located at the origin of the
// first shaft.
ChSharedPtr<ChLinkEngine> motor(new ChLinkEngine);
system.AddLink(motor);
motor->Initialize(ground, shaft_1, ChCoordsys<>(ChVector<>(0, 0, -hl), ChQuaternion<>(1, 0, 0, 0)));
motor->Set_eng_mode(ChLinkEngine::ENG_MODE_ROTATION);
motor->Set_rot_funct(ChSharedPtr<ChFunction>(new ChFunction_Ramp(0, 1)));
// Connect the second shaft to ground through a cylindrical joint
// --------------------------------------------------------------
// Use a cylindrical joint so that we do not have redundant constraints
// (note that, technically Chrono could deal with a revolute joint here).
// the joint is located at the origin of the second shaft.
ChSharedPtr<ChLinkLockCylindrical> cyljoint(new ChLinkLockCylindrical);
system.AddLink(cyljoint);
cyljoint->Initialize(ground, shaft_2, ChCoordsys<>(ChVector<>(0, -hl * sina, hl * cosa), rot));
// Connect the two shafts through a universal joint
// ------------------------------------------------
// The joint is located at the global origin. Its kinematic constraints will
// enforce orthogonality of the associated cross.
ChSharedPtr<ChLinkUniversal> ujoint(new ChLinkUniversal);
system.AddLink(ujoint);
ujoint->Initialize(shaft_1, shaft_2, ChFrame<>(ChVector<>(0, 0, 0), rot));
// Create the Irrlicht application
// -------------------------------
ChIrrApp application(&system, L"ChBodyAuxRef demo", core::dimension2d<u32>(800, 600), false, true);
application.AddTypicalLogo();
application.AddTypicalSky();
application.AddTypicalLights();
application.AddTypicalCamera(core::vector3df(3, 1, -1.5));
application.AssetBindAll();
application.AssetUpdateAll();
// Simulation loop
application.SetTimestep(0.001);
int frame = 0;
while (application.GetDevice()->run())
{
application.BeginScene();
application.DrawAll();
application.DoStep();
application.EndScene();
frame++;
if (frame % 20 == 0) {
// Output the shaft angular velocities at the current time
double omega_1 = shaft_1->GetWvel_loc().z;
double omega_2 = shaft_2->GetWvel_loc().z;
GetLog() << system.GetChTime() << " " << omega_1 << " " << omega_2 << "\n";
}
}
return 0;
}
void ChPythonEngine::ImportSolidWorksSystem(const char* solidworks_py_file, ChSystem& msystem) throw(ChException)
{
std::ostringstream sstream;
//sstream << "from " << std::string(solidworks_py_file) << " import exported_items\n";
sstream << "import builtins \n";
sstream << "import imp \n";
sstream << "import os \n";
sstream << "mdirname, mmodulename= os.path.split('" << std::string(solidworks_py_file) << "') \n";
sstream << "builtins.exported_system_relpath = mdirname + '/' \n";
sstream << "fp, pathname, description = imp.find_module(mmodulename,[builtins.exported_system_relpath]) \n";
sstream << "try: \n";
sstream << " imported_mod = imp.load_module('imported_mod', fp, pathname, description) \n";
sstream << "finally: \n";
sstream << " if fp: \n";
sstream << " fp.close() \n";
sstream << "exported_items = imported_mod.exported_items \n";
this->Run(sstream.str().c_str());
PyObject * module = PyImport_AddModule("__main__"); // borrowed reference
if (!module)
throw ChException("ERROR. No Python __main__ module?");
PyObject * dictionary = PyModule_GetDict(module); // borrowed reference
if (!dictionary)
throw ChException("ERROR. No Python dictionary?");
PyObject * result = PyDict_GetItemString(dictionary, "exported_items"); // borrowed reference
if (!result)
throw ChException("ERROR. Missing Python object 'exported_items' in SolidWorks file");
if (PyList_Check(result))
{
int nitems = PyList_Size(result);
//GetLog() << "N.of list items: " << nitems << "\n";
for (int i = 0; i< nitems; i++)
{
PyObject* mobj = PyList_GetItem(result,i);
if (mobj)
{
// GetLog() << " Python type: " << mobj->ob_type->tp_name << "\n";
SwigPyObject * mswigobj = SWIG_Python_GetSwigThis(mobj);
if (mswigobj)
{
void* objptr = mswigobj->ptr;
ChSharedPtr<ChPhysicsItem>* pt_to_shp = (ChSharedPtr<ChPhysicsItem>*)objptr;
/// Add the ChPhysicsItem to the ChSystem
msystem.Add( (*pt_to_shp) );
}
else
{
throw ChException("ERROR. Only shared pointers to ChPhysicsItem subclasses can be inside exported_items.");
}
}
}
msystem.Setup();
msystem.Update();
}
else
{
throw ChException("ERROR. exported_items python object is not a list.");
}
}
int main(int argc, char* argv[]) {
// Create a ChronoENGINE physical system
ChSystem mphysicalSystem;
// Create the Irrlicht visualization (open the Irrlicht device,
// bind a simple user interface, etc. etc.)
ChIrrApp application(&mphysicalSystem, L"Meshless deformable material", core::dimension2d<u32>(800, 600), false);
// Easy shortcuts to add camera, lights, logo and sky in Irrlicht scene:
ChIrrWizard::add_typical_Logo(application.GetDevice());
ChIrrWizard::add_typical_Sky(application.GetDevice());
ChIrrWizard::add_typical_Lights(application.GetDevice());
ChIrrWizard::add_typical_Camera(application.GetDevice(), core::vector3df(0, 3, -3));
// CREATE A FLOOR
ChSharedPtr<ChBodyEasyBox> mfloorBody( new ChBodyEasyBox(20,1,20,1000,true,true));
my_system.Add(mfloorBody);
mfloorBody->SetBodyFixed(true);
mfloorBody->SetPos(ChVector<>(0, -5, 0));
ChSharedPtr<ChTexture> mtexture( new ChTexture(GetChronoDataFile("concrete.jpg").c_str()));
mfloorBody->AddAsset(mtexture);
// CREATE THE ELASTOPLASTIC MESHLESS CONTINUUM
// Create elastoplastic matter
ChSharedPtr<ChMatterMeshless> mymatter(new ChMatterMeshless);
// Use the FillBox easy way to create the set of nodes in the meshless matter
mymatter->FillBox(ChVector<>(4, 2, 4), // size of box
4.0 / 10.0, // resolution step
1000, // initial density
ChCoordsys<>(ChVector<>(0, -3.9, 0), QUNIT), // position & rotation of box
true, // do a centered cubic lattice initial arrangement
2.1); // set the kernel radius (as multiples of step)
GetLog() << "Added " << mymatter->GetNnodes() << " nodes \n";
// Set some material properties of the meshless matter
ChSharedPtr<ChContinuumDruckerPrager> mmaterial(new ChContinuumDruckerPrager);
mymatter->ReplaceMaterial(mmaterial);
mmaterial->Set_v(0.35);
mmaterial->Set_E(30000.0);
mmaterial->Set_elastic_yeld(0);
mmaterial->Set_alpha(30 * CH_C_DEG_TO_RAD);
// mmaterial->Set_from_MohrCoulomb(30 * CH_C_DEG_TO_RAD, 1000);
mmaterial->Set_dilatancy(30 * CH_C_DEG_TO_RAD);
mmaterial->Set_flow_rate(50000000.0);
mmaterial->Set_hardening_limit(mmaterial->Get_elastic_yeld());
mmaterial->Set_hardening_speed(100000000);
/*
ChSharedPtr<ChContinuumPlasticVonMises> mmaterial(new ChContinuumPlasticVonMises);
mymatter->ReplaceMaterial(mmaterial);
mmaterial->Set_v(0.38);
mmaterial->Set_E(60000.0);
mmaterial->Set_elastic_yeld(0.06);
mmaterial->Set_flow_rate(300);
*/
mymatter->SetViscosity(5000);
// Add the matter to the physical system
mymatter->SetCollide(true);
mphysicalSystem.Add(mymatter);
// Join some nodes of meshless matter to a ChBody
/*
ChSharedPtr<ChIndexedNodes> mnodes = mymatter;
for (int ij = 0; ij < 120; ij++)
{
ChSharedPtr<ChLinkPointFrame> myjointnodebody(new ChLinkPointFrame);
myjointnodebody->Initialize(mnodes,
ij,
mfloorBody->GetBody());
mphysicalSystem.Add(myjointnodebody);
}
*/
// IMPORTANT!
// This takes care of the interaction between the particles of the meshless material
ChSharedPtr<ChProximityContainerMeshless> my_sph_proximity(new ChProximityContainerMeshless);
mphysicalSystem.Add(my_sph_proximity);
// CREATE A SPHERE PRESSING THE MATERIAL:
ChSharedPtr<ChBodyEasySphere> msphere( new ChBodyEasySphere(2, 7000, true,true));
my_system.Add(msphere);
msphere->SetPos(ChVector<>(0, -0.5, 0));
// Use this function for adding a ChIrrNodeAsset to all items
// Otherwise use application.AssetBind(myitem); on a per-item basis.
application.AssetBindAll();
// Use this function for 'converting' assets into Irrlicht meshes
application.AssetUpdateAll();
// Modify some setting of the physical system for the simulation, if you want
mphysicalSystem.SetMaxItersSolverSpeed(25); // lower the LCP iters, no needed here
//
// THE SOFT-REAL-TIME CYCLE
//
static int printed_prox = 0;
application.SetStepManage(true);
application.SetTimestep(0.002);
while (application.GetDevice()->run()) {
application.BeginScene(true, true, SColor(255, 140, 161, 192));
application.DrawAll();
ChSystem::IteratorOtherPhysicsItems myiter = mphysicalSystem.IterBeginOtherPhysicsItems();
while (myiter != mphysicalSystem.IterEndOtherPhysicsItems()) {
if ((*myiter).IsType<ChMatterMeshless>()) {
ChSharedPtr<ChMatterMeshless> mymatter((*myiter).DynamicCastTo<ChMatterMeshless>());
for (unsigned int ip = 0; ip < mymatter->GetNnodes(); ip++) {
ChSharedPtr<ChNodeMeshless> mnode(mymatter->GetNode(ip).DynamicCastTo<ChNodeMeshless>());
ChVector<> mv = mnode->GetPos();
float rad = (float)mnode->GetKernelRadius();
core::vector3df mpos((irr::f32)mv.x, (irr::f32)mv.y, (irr::f32)mv.z);
core::position2d<s32> spos =
application.GetSceneManager()->getSceneCollisionManager()->getScreenCoordinatesFrom3DPosition(
mpos);
application.GetVideoDriver()->draw2DRectangle(
video::SColor(100, 200, 200, 230),
core::rect<s32>(spos.X - 2, spos.Y - 2, spos.X + 2, spos.Y + 2));
/*
application.GetVideoDriver()->setTransform(video::ETS_WORLD, core::matrix4());
application.GetVideoDriver()->draw3DBox( core::aabbox3d<f32>(
(irr::f32)mv.x-rad ,(irr::f32)mv.y-rad , (irr::f32)mv.z-rad ,
(irr::f32)mv.x+rad ,(irr::f32)mv.y+rad , (irr::f32)mv.z+rad ) ,
video::SColor(300,200,200,230) );
*/
/*
double strain_scale =10;
ChIrrTools::drawSegment(application.GetVideoDriver(), mnode->GetPos(),
mnode->GetPos()+(VECT_X*mnode->p_strain.XX()* strain_scale), video::SColor(255,255,0,0),false);
ChIrrTools::drawSegment(application.GetVideoDriver(), mnode->GetPos(),
mnode->GetPos()+(VECT_Y*mnode->p_strain.YY()* strain_scale), video::SColor(255,0,255,0),false);
ChIrrTools::drawSegment(application.GetVideoDriver(), mnode->GetPos(),
mnode->GetPos()+(VECT_Z*mnode->p_strain.ZZ()* strain_scale), video::SColor(255,0,0,255),false);
*/
/*
double stress_scale =0.008;
ChIrrTools::drawSegment(application.GetVideoDriver(), mnode->GetPos(),
mnode->GetPos()+(VECT_X*mnode->e_stress.XX()* stress_scale), video::SColor(100,255,0,0),false);
ChIrrTools::drawSegment(application.GetVideoDriver(), mnode->GetPos(),
mnode->GetPos()+(VECT_Y*mnode->e_stress.YY()* stress_scale), video::SColor(100,0,255,0),false);
ChIrrTools::drawSegment(application.GetVideoDriver(), mnode->GetPos(),
mnode->GetPos()+(VECT_Z*mnode->e_stress.ZZ()* stress_scale), video::SColor(100,0,0,255),false);
*/
double stress_scale = 0.00001;
ChIrrTools::drawSegment(
application.GetVideoDriver(), mnode->GetPos(),
mnode->GetPos() + (VECT_X * mnode->e_stress.GetInvariant_I1() * stress_scale),
video::SColor(100, 255, 0, 0), false);
// GetLog() << "Mass i="<< ip << " m=" << mnode->GetMass() << "\n";
// ChIrrTools::drawSegment(application.GetVideoDriver(), mnode->GetPos(),
// mnode->GetPos()+(mnode->UserForce * 0.1), video::SColor(100,0,0,0),false);
}
}
++myiter;
}
application.DoStep();
application.EndScene();
}
return 0;
}
// Delete the car object, deleting also all bodies corresponding to
// the various parts and removing them from the physical system. Also
// removes constraints from the system.
MySimpleCar::~MySimpleCar()
{
ChSystem* mysystem = spindleRF->GetBody()->GetSystem(); // trick to get the system here
// When a ChBodySceneNode is removed via ->remove() from Irrlicht 3D scene manager,
// it is also automatically removed from the ChSystem (the ChSystem::RemoveBody() is
// automatically called at Irrlicht node deletion - see ChBodySceneNode.h ).
// For links, just remove them from the ChSystem using ChSystem::RemoveLink()
mysystem->RemoveLink(link_revoluteRF);
mysystem->RemoveLink(link_brakeRF);
mysystem->RemoveLink(link_distRFU1);
mysystem->RemoveLink(link_distRFU2);
mysystem->RemoveLink(link_distRFL1);
mysystem->RemoveLink(link_distRFL2);
mysystem->RemoveLink(link_springRF);
mysystem->RemoveLink(link_distRSTEER);
mysystem->RemoveLink(link_revoluteLF);
mysystem->RemoveLink(link_brakeLF);
mysystem->RemoveLink(link_distLFU1);
mysystem->RemoveLink(link_distLFU2);
mysystem->RemoveLink(link_distLFL1);
mysystem->RemoveLink(link_distLFL2);
mysystem->RemoveLink(link_springLF);
mysystem->RemoveLink(link_distLSTEER);
mysystem->RemoveLink(link_revoluteRB);
mysystem->RemoveLink(link_brakeRB);
mysystem->RemoveLink(link_distRBU1);
mysystem->RemoveLink(link_distRBU2);
mysystem->RemoveLink(link_distRBL1);
mysystem->RemoveLink(link_distRBL2);
mysystem->RemoveLink(link_springRB);
mysystem->RemoveLink(link_distRBlat);
mysystem->RemoveLink(link_engineR);
mysystem->RemoveLink(link_revoluteLB);
mysystem->RemoveLink(link_brakeLB);
mysystem->RemoveLink(link_distLBU1);
mysystem->RemoveLink(link_distLBU2);
mysystem->RemoveLink(link_distLBL1);
mysystem->RemoveLink(link_distLBL2);
mysystem->RemoveLink(link_springLB);
mysystem->RemoveLink(link_distLBlat);
mysystem->RemoveLink(link_engineL);
truss->remove();
spindleRF->remove();
wheelRF->remove();
spindleLF->remove();
wheelLF->remove();
spindleRB->remove();
wheelRB->remove();
spindleLB->remove();
wheelLB->remove();
sound_engine->drop();
}
MySimpleCar::MySimpleCar(ChSystem& my_system, ///< the chrono::engine physical system
ISceneManager* msceneManager, ///< the Irrlicht scene manager for 3d shapes
IVideoDriver* mdriver ///< the Irrlicht video driver
)
{
throttle = 0; // initially, gas throttle is 0.
conic_tau = 0.2;
gears[0] = -0.3; // rear gear<
gears[1] = 0.3; // 1st gear
gears[2] = 0.4; // 2nd gear
gears[3] = 0.6; // 3rd gear
gears[4] = 0.8; // 4th gear
gears[5] = 1.0; // 5th gear
actual_gear = 1;
gear_tau = gears[actual_gear];
// Define the torque curve with some polygonal approximation, entering
// a sequence of x-y pairs (speed-torque, with speed in [rad/s] and torque in [Nm])
// (Of course a real engine should have not-null torque only from some positive value, but
// here we start from -50 rad/s otherwise we cannot start - given we don't model the clutch)
torque_curve.AddPoint(-50, 35);
torque_curve.AddPoint(0, 45);
torque_curve.AddPoint(400,50);
torque_curve.AddPoint(700,54);
torque_curve.AddPoint(850, 0);
torque_curve.AddPoint(1000, -54); // finish in 4th quadrant
torque_curve.AddPoint(2000, -60); // finish in 4th quadrant
ChStreamOutAsciiFile myfiletq("data_torquecurve.dat");
torque_curve.FileAsciiPairsSave(myfiletq,-100,1000,200);
motorspeed= motortorque=0;
passo = 1.73162;
carr = 0.59963;
wanted_steer = 0;
actual_steer = 0;
max_steer_speed = 0.5;
convergenza_anteriore = 0;
convergenza_posteriore = 0;
max_brake_torque_post = 200;
max_brake_torque_ant = 300;
braking = 0;
// --- The car body ---
// Set the position of the center of gravity of the truss, respect to the auxiliary reference that
// we will use to measure all relative positions of joints on the truss.
truss_COG.Set( 0, 0, 0);
truss = (ChBodySceneNode*)addChBodySceneNode(
&my_system, msceneManager, 0,
150.0,
ChVector<>(0, 0.016 ,1.99) +truss_COG,
QUNIT);
truss->GetBody()->SetInertiaXX(ChVector<>(4.8, 4.5, 1));
truss->GetBody()->SetBodyFixed(false);
// Add some 'invisible' boxes which roughly define the collision shape of
// the car truss. Each colliding box must be defined in x y z half-sizes and position of
// box center respect to the truss COG.
truss->GetBody()->GetCollisionModel()->ClearModel();
truss->GetBody()->GetCollisionModel()->AddBox(0.3, 0.3, 1.5, &(ChVector<>(0,0.2,0)-truss_COG) ); // just example.. roughly the body
truss->GetBody()->GetCollisionModel()->AddBox(0.1, 0.3, 0.1, &(ChVector<>(0,0.4,0)-truss_COG) ); // just example.. roughly the rollbar
truss->GetBody()->GetCollisionModel()->BuildModel();
truss->GetBody()->SetCollide(true);
// Add a visible mesh for Irrlicht representation of the
// car - it must be found on disk with the relative path as indicated..
IAnimatedMesh* scocca_mesh = msceneManager->getMesh("../data/mycar.obj");
IAnimatedMeshSceneNode* scocca_node = msceneManager->addAnimatedMeshSceneNode( scocca_mesh, truss );
scocca_node->setPosition(vector3dfCH(-truss_COG)); // if COG is not the reference we use for all stuff in truss..
scocca_node->addShadowVolumeSceneNode();
// --- Right Front suspension ---
// ..the car right-front spindle
spindleRF = (ChBodySceneNode*)addChBodySceneNode_easyBox(
&my_system, msceneManager,
8.0,// mass of the spindle
ChVector<>(-(carr-0.05216), 0.02865 ,0.93534),
QUNIT,
ChVector<>(0.05, 0.22, 0.16) );
spindleRF->GetBody()->SetInertiaXX(ChVector<>(0.2, 0.2, 0.2));
spindleRF->GetBody()->SetCollide(false);
// ..the car right-front wheel
wheelRF = (ChBodySceneNode*)addChBodySceneNode_easyCylinder(
&my_system, msceneManager,
3.0,
ChVector<>(-carr ,0.02865 ,0.93534),
chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Z), //rotate, otherwise flat,horizontal
ChVector<>(0.42, 0.17, 0.42) );
wheelRF->GetBody()->SetInertiaXX(ChVector<>(0.2, 0.2, 0.2));
wheelRF->GetBody()->SetCollide(true);
wheelRF->GetBody()->SetFriction(1.3);;
wheelRF->GetBody()->SetName("wheelRF");
video::ITexture* cylinderMap = mdriver->getTexture("../data/bluwhite.png");
wheelRF->setMaterialTexture(0, cylinderMap);
wheelRF->addShadowVolumeSceneNode();
// .. create the revolute joint between the wheel and the spindle
// Note that we specify two ChCoordinates of two markers which will be added in the two bodies,
// but both rotated with Z their axes pointing laterally thank to the Q_from_AngAxis() 90 degrees
// rotations if necessary, because ChLinkLockRevolute will use the Z axes of markers as shafts.
link_revoluteRF = ChSharedPtr<ChLinkLockRevolute>(new ChLinkLockRevolute); // right, front, upper, 1
link_revoluteRF->Initialize(wheelRF->GetBody(), spindleRF->GetBody(),
true, // 'true' means: following two positions are relative to wheel and body
ChCoordsys<>(ChVector<>(0,0,0) , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_X)) , // shaft in wheel coords
ChCoordsys<>(ChVector<>(-0.05216,0,0) , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Y)) ); // shaft in spindle coords
my_system.AddLink(link_revoluteRF);
// .. create the brake between the wheel and the spindle, using the same markers already created for wheel-spindle revolute joint:
link_brakeRF = ChSharedPtr<ChLinkBrake>(new ChLinkBrake);
link_revoluteRF->GetMarker1()->AddRef();
link_revoluteRF->GetMarker2()->AddRef();
link_brakeRF->Initialize(ChSharedMarkerPtr(link_revoluteRF->GetMarker1()),
ChSharedMarkerPtr(link_revoluteRF->GetMarker2()) );
my_system.AddLink(link_brakeRF);
// .. impose distance between two parts (as a massless rod with two spherical joints at the end)
link_distRFU1 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right, front, upper, 1
link_distRFU1->Initialize(truss->GetBody(), spindleRF->GetBody(),
true, // false = positions are relative to COG csystems of bodies, true = absolute positions
ChVector<>(-0.24708, 0.09313, -1.17789)-truss_COG, // position of 1st end rod in truss COG coordinate
ChVector<>( 0.04263,0.0925,0), // position of 2nd end rod in spindle COG coordinates
false, 0.2895 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distRFU1);
link_distRFU2 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right, front, upper, 2
link_distRFU2->Initialize(truss->GetBody(), spindleRF->GetBody(),
true,
ChVector<>(-0.24708,0.09313,-0.71972)-truss_COG,
ChVector<>( 0.04263,0.0925,0),
false, 0.4228 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distRFU2);
link_distRFL1 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right, front, lower, 1
link_distRFL1->Initialize(truss->GetBody(), spindleRF->GetBody(),
true,
ChVector<>(-0.24708,-0.10273,-1.17789)-truss_COG,
ChVector<>( 0.04263,-0.1225,0),
false, 0.2857 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distRFL1);
link_distRFL2 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right, front, lower, 2
link_distRFL2->Initialize(truss->GetBody(), spindleRF->GetBody(),
true,
ChVector<>(-0.24708,-0.10273,-0.71972)-truss_COG,
ChVector<>( 0.04263,-0.1225,0),
false, 0.4227 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distRFL2);
// .. create the spring between the truss and the spindle
link_springRF = ChSharedPtr<ChLinkSpring>(new ChLinkSpring);
link_springRF->Initialize(truss->GetBody(), spindleRF->GetBody(),
true,
ChVector<>(-0.31394,0.19399,-1.06243)-truss_COG,
ChVector<>( 0.06983,-0.08962,-0.00777),
false, 0.316); // no auto-distance computation: use imposed spring restlength
link_springRF->Set_SpringK(28300);
link_springRF->Set_SpringR(80);
my_system.AddLink(link_springRF);
// .. create the rod for steering the wheel
link_distRSTEER = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right steer
link_distRSTEER->Initialize(truss->GetBody(), spindleRF->GetBody(),
true,
ChVector<>(-0.25457,0.10598,-1.24879)-truss_COG,
ChVector<>( 0.06661,0.09333,-0.15003),
false, 0.2330); // no auto-distance computation: use imposed distance (rod length)
my_system.AddLink(link_distRSTEER);
// --- Left Front suspension ---
// ..the car left-front spindle
spindleLF = (ChBodySceneNode*)addChBodySceneNode_easyBox(
&my_system, msceneManager,
8.0,// mass of the spindle
ChVector<>( (carr-0.05216), 0.02865 ,0.93534),
QUNIT,
ChVector<>(0.05, 0.22, 0.16) );
spindleLF->GetBody()->SetInertiaXX(ChVector<>(0.2, 0.2, 0.2));
spindleLF->GetBody()->SetCollide(false);
// ..the car left-front wheel
wheelLF = (ChBodySceneNode*)addChBodySceneNode_easyCylinder(
&my_system, msceneManager,
3.0,
ChVector<>( carr ,0.02865 ,0.93534),
chrono::Q_from_AngAxis(-CH_C_PI/2, VECT_Z), //rotate, otherwise flat,horizontal
ChVector<>(0.42, 0.17, 0.42) );
wheelLF->GetBody()->SetInertiaXX(ChVector<>(0.2, 0.2, 0.2));
wheelLF->GetBody()->SetCollide(true);
wheelLF->GetBody()->SetFriction(1.3);
wheelLF->GetBody()->SetName("wheelLF");
wheelLF->setMaterialTexture(0, cylinderMap);
wheelLF->addShadowVolumeSceneNode();
// .. create the revolute joint between the wheel and the spindle
link_revoluteLF = ChSharedPtr<ChLinkLockRevolute>(new ChLinkLockRevolute); // left, front, upper, 1
link_revoluteLF->Initialize(wheelLF->GetBody(), spindleLF->GetBody(),
true, // 'true' means: following two positions are relative to wheel and body
ChCoordsys<>(ChVector<>(0,0,0) , chrono::Q_from_AngAxis(-CH_C_PI/2, VECT_X)) , // shaft in wheel coords
ChCoordsys<>(ChVector<>( 0.05216,0,0) , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Y)) ); // shaft in spindle coords
my_system.AddLink(link_revoluteLF);
// .. create the brake between the wheel and the spindle, using the same markers already created for wheel-spindle revolute joint:
link_brakeLF = ChSharedPtr<ChLinkBrake>(new ChLinkBrake);
link_revoluteLF->GetMarker1()->AddRef();
link_revoluteLF->GetMarker2()->AddRef();
link_brakeLF->Initialize(ChSharedMarkerPtr(link_revoluteLF->GetMarker1()),
ChSharedMarkerPtr(link_revoluteLF->GetMarker2()) );
my_system.AddLink(link_brakeLF);
// .. impose distance between two parts (as a massless rod with two spherical joints at the end)
link_distLFU1 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // left, front, upper, 1
link_distLFU1->Initialize(truss->GetBody(), spindleLF->GetBody(),
true, // false = positions are relative to COG csystems of bodies, true = absolute positions
ChVector<>( 0.24708, 0.09313, -1.17789)-truss_COG, // position of 1st end rod in truss COG coordinate
ChVector<>(-0.04263,0.0925,0), // position of 2nd end rod in spindle COG coordinates
false, 0.2895 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distLFU1);
link_distLFU2 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // left, front, upper, 2
link_distLFU2->Initialize(truss->GetBody(), spindleLF->GetBody(),
true,
ChVector<>( 0.24708,0.09313, -0.71972)-truss_COG,
ChVector<>(-0.04263,0.0925,0),
false, 0.4228 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distLFU2);
link_distLFL1 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // left, front, lower, 1
link_distLFL1->Initialize(truss->GetBody(), spindleLF->GetBody(),
true,
ChVector<>( 0.24708,-0.10273,-1.17789)-truss_COG,
ChVector<>(-0.04263,-0.1225,0),
false, 0.2857 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distLFL1);
link_distLFL2 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // left, front, lower, 2
link_distLFL2->Initialize(truss->GetBody(), spindleLF->GetBody(),
true,
ChVector<>( 0.24708,-0.10273,-0.71972)-truss_COG,
ChVector<>(-0.04263,-0.1225,0),
false, 0.4227 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distLFL2);
// .. create the spring between the truss and the spindle
link_springLF = ChSharedPtr<ChLinkSpring>(new ChLinkSpring);
link_springLF->Initialize(truss->GetBody(), spindleLF->GetBody(),
true,
ChVector<>( 0.31394,0.19399,-1.06243)-truss_COG,
ChVector<>(-0.06983,-0.08962,-0.00777),
false, 0.316); // no auto-distance computation: use imposed spring restlength
link_springLF->Set_SpringK(28300);
link_springLF->Set_SpringR(80);
my_system.AddLink(link_springLF);
// .. create the rod for steering the wheel
link_distLSTEER = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right steer
link_distLSTEER->Initialize(truss->GetBody(), spindleLF->GetBody(),
true,
ChVector<>( 0.25457,0.10598,-1.24879)-truss_COG,
ChVector<>(-0.06661,0.09333,-0.15003),
false, 0.2330); // no auto-distance computation: use imposed distance (rod length)
my_system.AddLink(link_distLSTEER);
// --- Right Back suspension ---
// ..the car right-back spindle
spindleRB = (ChBodySceneNode*)addChBodySceneNode_easyBox(
&my_system, msceneManager,
8.0,
ChVector<>(-(carr-0.05216), 0.02865 ,(0.93534+passo)),
QUNIT,
ChVector<>(0.05, 0.22, 0.16) );
spindleRB->GetBody()->SetInertiaXX(ChVector<>(0.2, 0.2, 0.2));
spindleRB->GetBody()->SetCollide(false);
// ..the car right-back wheel
wheelRB = (ChBodySceneNode*)addChBodySceneNode_easyCylinder(
&my_system, msceneManager,
3.0,
ChVector<>(-carr ,0.02865 ,(0.93534+passo)),
chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Z), //rotate, otherwise flat,horizontal
ChVector<>(0.42, 0.17, 0.42) );
wheelRB->GetBody()->SetInertiaXX(ChVector<>(1.2, 1.2, 1.2));
wheelRB->GetBody()->SetCollide(true);
wheelRB->GetBody()->SetFriction(1.3);
wheelRB->GetBody()->SetName("wheelRB");
cylinderMap = mdriver->getTexture("../data/bluwhite.png");
wheelRB->setMaterialTexture(0, cylinderMap);
wheelRB->addShadowVolumeSceneNode();
// .. create the revolute joint between the wheel and the spindle
link_revoluteRB = ChSharedPtr<ChLinkLockRevolute>(new ChLinkLockRevolute); // right, back, upper, 1
link_revoluteRB->Initialize(wheelRB->GetBody(), spindleRB->GetBody(),
true, // 'true' means: following two positions are relative to wheel and body
ChCoordsys<>(ChVector<>(0,0,0) , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_X)) , // shaft in wheel coords
ChCoordsys<>(ChVector<>(-0.05216,0,0) , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Y)) ); // shaft in spindle coords
my_system.AddLink(link_revoluteRB);
// .. create the brake between the wheel and the spindle, using the same markers already created for wheel-spindle revolute joint:
link_brakeRB = ChSharedPtr<ChLinkBrake>(new ChLinkBrake);
link_revoluteRB->GetMarker1()->AddRef();
link_revoluteRB->GetMarker2()->AddRef();
link_brakeRB->Initialize(ChSharedMarkerPtr(link_revoluteRB->GetMarker1()),
ChSharedMarkerPtr(link_revoluteRB->GetMarker2()) );
my_system.AddLink(link_brakeRB);
// .. create the motor transmission joint between the wheel and the truss (assuming small changes of alignment)
link_engineR = ChSharedPtr<ChLinkEngine>(new ChLinkEngine);
link_engineR->Initialize(wheelRB->GetBody(), truss->GetBody(),
ChCoordsys<>(ChVector<>(-carr,0.02865,(0.93534+passo)) , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Y)) );
link_engineR->Set_shaft_mode(ChLinkEngine::ENG_SHAFT_CARDANO); // approx as a double Rzeppa joint
link_engineR->Set_eng_mode(ChLinkEngine::ENG_MODE_TORQUE);
my_system.AddLink(link_engineR);
// .. impose distance between two parts (as a massless rod with two spherical joints at the end)
link_distRBU1 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right, back, upper, 1
link_distRBU1->Initialize(truss->GetBody(), spindleRB->GetBody(),
true,
ChVector<>(-0.24708, 0.09313, -1.17789+passo)-truss_COG,
ChVector<>( 0.04263,0.0925,0), // position of 2nd end rod in spindle COG coordinates
false, 0.2895 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distRBU1);
link_distRBU2 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right, back, upper, 2
link_distRBU2->Initialize(truss->GetBody(), spindleRB->GetBody(),
true,
ChVector<>(-0.24708,0.09313,-0.71972+passo)-truss_COG,
ChVector<>( 0.04263,0.0925,0),
false, 0.4228 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distRBU2);
link_distRBL1 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right, back, lower, 1
link_distRBL1->Initialize(truss->GetBody(), spindleRB->GetBody(),
true,
ChVector<>(-0.24708,-0.10273,-1.17789+passo)-truss_COG,
ChVector<>( 0.04263,-0.1225,0),
false, 0.2857 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distRBL1);
link_distRBL2 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right, back, lower, 2
link_distRBL2->Initialize(truss->GetBody(), spindleRB->GetBody(),
true,
ChVector<>(-0.24708,-0.10273,-0.71972+passo)-truss_COG,
ChVector<>( 0.04263,-0.1225,0),
false, 0.4227 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distRBL2);
// .. create the spring between the truss and the spindle
link_springRB = ChSharedPtr<ChLinkSpring>(new ChLinkSpring);
link_springRB->Initialize(truss->GetBody(), spindleRB->GetBody(),
true,
ChVector<>(-0.31394,0.19399,-1.06243+passo)-truss_COG,
ChVector<>( 0.06983,-0.08962,-0.00777),
false, 0.316); // no auto-distance computation: use imposed spring restlength
link_springRB->Set_SpringK(28300);
link_springRB->Set_SpringR(80);
my_system.AddLink(link_springRB);
// .. create the rod for avoid the steering of the wheel
link_distRBlat = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right rod
link_distRBlat->Initialize(truss->GetBody(), spindleRB->GetBody(),
true,
ChVector<>(-0.25457,0.10598,-1.24879+passo)-truss_COG,
ChVector<>( 0.06661,0.09333,-0.10003),
false, 0.2450); // no auto-distance computation: use imposed distance (rod length)
my_system.AddLink(link_distRBlat);
// --- Left Back suspension ---
// ..the car right-back spindle
spindleLB = (ChBodySceneNode*)addChBodySceneNode_easyBox(
&my_system, msceneManager,
8.0,
ChVector<>((carr-0.05216), 0.02865 ,(0.93534+passo)),
QUNIT,
ChVector<>(0.05, 0.22, 0.16) );
spindleLB->GetBody()->SetInertiaXX(ChVector<>(0.2, 0.2, 0.2));
spindleLB->GetBody()->SetCollide(false);
// ..the car left-back wheel
wheelLB = (ChBodySceneNode*)addChBodySceneNode_easyCylinder(
&my_system, msceneManager,
3.0,
ChVector<>(carr ,0.02865 ,(0.93534+passo)),
chrono::Q_from_AngAxis(-CH_C_PI/2, VECT_Z), //rotate, otherwise flat,horizontal
ChVector<>(0.42, 0.17, 0.42) );
wheelLB->GetBody()->SetInertiaXX(ChVector<>(1.2, 1.2, 1.2));
wheelLB->GetBody()->SetCollide(true);
wheelLB->GetBody()->SetFriction(1.3);
wheelLB->GetBody()->SetName("wheelLB");
wheelLB->setMaterialTexture(0, cylinderMap);
wheelLB->addShadowVolumeSceneNode();
// .. create the revolute joint between the wheel and the spindle
link_revoluteLB = ChSharedPtr<ChLinkLockRevolute>(new ChLinkLockRevolute); // left, back, upper, 1
link_revoluteLB->Initialize(wheelLB->GetBody(), spindleLB->GetBody(),
true, // 'true' means: following two positions are relative to wheel and body
ChCoordsys<>(ChVector<>(0,0,0) , chrono::Q_from_AngAxis(-CH_C_PI/2, VECT_X)) , // shaft in wheel coords
ChCoordsys<>(ChVector<>(0.05216,0,0) , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Y)) ); // shaft in spindle coords
my_system.AddLink(link_revoluteLB);
// .. create the brake between the wheel and the spindle, using the same markers already created for wheel-spindle revolute joint:
link_brakeLB = ChSharedPtr<ChLinkBrake>(new ChLinkBrake);
link_revoluteLB->GetMarker1()->AddRef();
link_revoluteLB->GetMarker2()->AddRef();
link_brakeLB->Initialize(ChSharedMarkerPtr(link_revoluteLB->GetMarker1()),
ChSharedMarkerPtr(link_revoluteLB->GetMarker2()) );
my_system.AddLink(link_brakeLB);
// .. create the motor transmission joint between the wheel and the truss (assuming small changes of alignment)
link_engineL = ChSharedPtr<ChLinkEngine>(new ChLinkEngine);
link_engineL->Initialize(wheelLB->GetBody(), truss->GetBody(),
ChCoordsys<>(ChVector<>(carr,0.02865,(0.93534+passo)) , chrono::Q_from_AngAxis(CH_C_PI/2, VECT_Y)) );
link_engineL->Set_shaft_mode(ChLinkEngine::ENG_SHAFT_CARDANO); // approx as a double Rzeppa joint
link_engineL->Set_eng_mode(ChLinkEngine::ENG_MODE_TORQUE);
my_system.AddLink(link_engineL);
// .. impose distance between two parts (as a massless rod with two spherical joints at the end)
link_distLBU1 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // left, back, upper, 1
link_distLBU1->Initialize(truss->GetBody(), spindleLB->GetBody(),
true,
ChVector<>( 0.24708, 0.09313, -1.17789+passo)-truss_COG,
ChVector<>(-0.04263,0.0925,0), // position of 2nd end rod in spindle COG coordinates
false, 0.2895 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distLBU1);
link_distLBU2 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // left, back, upper, 2
link_distLBU2->Initialize(truss->GetBody(), spindleLB->GetBody(),
true,
ChVector<>( 0.24708,0.09313, -0.71972+passo)-truss_COG,
ChVector<>(-0.04263,0.0925,0),
false, 0.4228 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distLBU2);
link_distLBL1 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // left, back, lower, 1
link_distLBL1->Initialize(truss->GetBody(), spindleLB->GetBody(),
true,
ChVector<>( 0.24708,-0.10273,-1.17789+passo)-truss_COG,
ChVector<>(-0.04263,-0.1225,0),
false, 0.2857 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distLBL1);
link_distLBL2 = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // left, back, lower, 2
link_distLBL2->Initialize(truss->GetBody(), spindleLB->GetBody(),
true,
ChVector<>( 0.24708,-0.10273,-0.71972+passo)-truss_COG,
ChVector<>(-0.04263,-0.1225,0),
false, 0.4227 ); // no auto-distance computation: use imposed distance (length of rod).
my_system.AddLink(link_distLBL2);
// .. create the spring between the truss and the spindle
link_springLB = ChSharedPtr<ChLinkSpring>(new ChLinkSpring);
link_springLB->Initialize(truss->GetBody(), spindleLB->GetBody(),
true,
ChVector<>( 0.31394,0.19399,-1.06243+passo)-truss_COG,
ChVector<>(-0.06983,-0.08962,-0.00777),
false, 0.316); // no auto-distance computation: use imposed spring restlength
link_springLB->Set_SpringK(28300);
link_springLB->Set_SpringR(80);
my_system.AddLink(link_springLB);
// .. create the rod for avoid the steering of the wheel
link_distLBlat = ChSharedPtr<ChLinkDistance>(new ChLinkDistance); // right
link_distLBlat->Initialize(truss->GetBody(), spindleLB->GetBody(),
true,
ChVector<>( 0.25457,0.10598,-1.24879+passo)-truss_COG,
ChVector<>(-0.06661,0.09333,-0.10003),
false, 0.2450); // no auto-distance computation: use imposed distance (rod length)
my_system.AddLink(link_distLBlat);
// Create also the motor sound
//
// start the sound engine with default parameters
sound_engine = irrklang::createIrrKlangDevice();
if (!sound_engine)
{
GetLog() << "Cannot start sound engine Irrklang \n";
}
// To play a sound, we only to call play2D(). The second parameter
// tells the engine to play it looped.
// play some sound stream, looped
if (sound_engine)
car_sound = sound_engine->play2D("../data/carsound.ogg", true, false, true);
else
car_sound = 0;
}
int main(int argc, char* argv[])
{
// Create a ChronoENGINE physical system
ChSystem mphysicalSystem;
// Create the Irrlicht visualization (open the Irrlicht device,
// bind a simple user interface, etc. etc.)
ChIrrApp application(&mphysicalSystem, L"Critical cases for solver convergence",core::dimension2d<u32>(800,600),false,true);
// Easy shortcuts to add camera, lights, logo and sky in Irrlicht scene:
ChIrrWizard::add_typical_Logo(application.GetDevice());
ChIrrWizard::add_typical_Sky(application.GetDevice());
ChIrrWizard::add_typical_Lights(application.GetDevice());
ChIrrWizard::add_typical_Camera(application.GetDevice(), core::vector3df(0,1.5,-3));
// Create all the rigid bodies.
create_items(application);
class MyContactCallback : public ChSystem::ChCustomCollisionPointCallback
{
public: virtual void ContactCallback(
const collision::ChCollisionInfo& mcontactinfo, ///< get info about contact (cannot change it)
ChMaterialCouple& material ) ///< you can modify this!
{
// Set compliance (normal and tangential at once)
material.compliance = STATIC_COMPLIANCE;
material.complianceT = material.compliance ;
material.dampingf = 0.2f;
};
ChSystem* msystem;
};
MyContactCallback mycontact_callback; // create the callback object
mycontact_callback.msystem = &mphysicalSystem; // will be used by callback
// Tell the system to use the callback above, per each created contact!
mphysicalSystem.SetCustomCollisionPointCallback(&mycontact_callback);
// Use this function for adding a ChIrrNodeAsset to all already created items (ex. a floor, a wall, etc.)
// Otherwise use application.AssetBind(myitem); on a per-item basis.
application.AssetBindAll();
application.AssetUpdateAll();
// Modify some setting of the physical system for the simulation, if you want
mphysicalSystem.SetLcpSolverType(ChSystem::LCP_ITERATIVE_BARZILAIBORWEIN);
//mphysicalSystem.SetLcpSolverType(ChSystem::LCP_ITERATIVE_SOR);
mphysicalSystem.SetIterLCPmaxItersSpeed(60);
mphysicalSystem.SetIterLCPmaxItersStab(5);
mphysicalSystem.SetParallelThreadNumber(1);
//mphysicalSystem.SetUseSleeping(true);
application.SetStepManage(true);
application.SetTimestep(0.01);
application.SetPaused(true);
//
// THE SOFT-REAL-TIME CYCLE
//
while(application.GetDevice()->run())
{
application.GetVideoDriver()->beginScene(true, true, SColor(255,140,161,192));
application.DrawAll();
align_spheres(application); // just to simplify test, on y axis only
application.DoStep();
application.GetVideoDriver()->endScene();
}
return 0;
}
void create_wall_bodies(ChSystem& mphysicalSystem)
{
// Create a material that will be shared between bricks
ChSharedPtr<ChMaterialSurface> mmaterial(new ChMaterialSurface);
mmaterial->SetFriction(0.4f);
mmaterial->SetCompliance (0.0000005f);
mmaterial->SetComplianceT(0.0000005f);
mmaterial->SetDampingF(0.2f);
// Create bricks
for (int ai = 0; ai < 1; ai++) // N. of walls
{
for (int bi = 0; bi < 10; bi++) // N. of vert. bricks
{
for (int ui = 0; ui < 15; ui++) // N. of hor. bricks
{
ChSharedPtr<ChBodyEasyBox> mrigidBody(new ChBodyEasyBox(
3.96, 2, 4, // x,y,z size
100, // density
true, // collide enable?
true)); // visualization?
mrigidBody->SetPos(ChVector<>(-8+ui*4.0+2*(bi%2), 1.0+bi*2.0, ai*9));
mrigidBody->SetMaterialSurface(mmaterial); // use shared surface properties
mphysicalSystem.Add(mrigidBody);
// optional, attach a texture for better visualization
ChSharedPtr<ChTexture> mtexture(new ChTexture());
mtexture->SetTextureFilename("../data/cubetexture_borders.png");
mrigidBody->AddAsset(mtexture);
}
}
}
// Create the floor using
// fixed rigid body of 'box' type:
ChSharedPtr<ChBodyEasyBox> mrigidFloor(new ChBodyEasyBox(
250, 4, 250, // x,y,z size
1000, // density
true, // collide enable?
true)); // visualization?
mrigidFloor->SetPos(ChVector<>(0,-2,0));
mrigidFloor->SetMaterialSurface(mmaterial);
mrigidFloor->SetBodyFixed(true);
mphysicalSystem.Add(mrigidFloor);
// Create a ball that will collide with wall
ChSharedPtr<ChBodyEasySphere> mrigidBall(new ChBodyEasySphere(
4, // radius
8000, // density
true, // collide enable?
true)); // visualization?
mrigidBall->SetPos(ChVector<>(0,-2,0));
mrigidBall->SetMaterialSurface(mmaterial);
mrigidBall->SetPos(ChVector<>(0, 3, -8));
mrigidBall->SetPos_dt(ChVector<>(0,0,16)); // set initial speed
mrigidBall->GetMaterialSurface()->SetFriction(0.4f); // use own (not shared) matrial properties
mrigidBall->GetMaterialSurface()->SetCompliance(0.0);
mrigidBall->GetMaterialSurface()->SetComplianceT(0.0);
mrigidBall->GetMaterialSurface()->SetDampingF(0.2f);
mphysicalSystem.Add(mrigidBall);
// optional, attach a texture for better visualization
ChSharedPtr<ChTexture> mtextureball(new ChTexture());
mtextureball->SetTextureFilename("../data/bluwhite.png");
mrigidBall->AddAsset(mtextureball);
}
int main(int argc, char* argv[])
{
// In CHRONO engine, The DLL_CreateGlobals() - DLL_DeleteGlobals(); pair is needed if
// global functions are needed.
DLL_CreateGlobals();
// Create a ChronoENGINE physical system
ChSystem mphysicalSystem;
// Create the Irrlicht visualization (open the Irrlicht device,
// bind a simple user interface, etc. etc.)
ChIrrApp application(&mphysicalSystem, L"Bricks test",core::dimension2d<u32>(800,600),false, true);
// Easy shortcuts to add camera, lights, logo and sky in Irrlicht scene:
ChIrrWizard::add_typical_Logo (application.GetDevice());
ChIrrWizard::add_typical_Sky (application.GetDevice());
ChIrrWizard::add_typical_Lights(application.GetDevice(), core::vector3df(70.f, 120.f, -90.f), core::vector3df(30.f, 80.f, 60.f), 290, 190);
ChIrrWizard::add_typical_Camera(application.GetDevice(), core::vector3df(-15,14,-30), core::vector3df(0,5,0));
//
// HERE YOU POPULATE THE MECHANICAL SYSTEM OF CHRONO...
//
// Create all the rigid bodies.
create_wall_bodies (mphysicalSystem);
//create_jengatower_bodies (mphysicalSystem);
// Use this function for adding a ChIrrNodeAsset to all items
// If you need a finer control on which item really needs a visualization proxy in
// Irrlicht, just use application.AssetBind(myitem); on a per-item basis.
application.AssetBindAll();
// Use this function for 'converting' into Irrlicht meshes the assets
// into Irrlicht-visualizable meshes
application.AssetUpdateAll();
// Prepare the physical system for the simulation
mphysicalSystem.SetLcpSolverType(ChSystem::LCP_ITERATIVE_SOR_MULTITHREAD);
mphysicalSystem.SetUseSleeping(false);
mphysicalSystem.SetMaxPenetrationRecoverySpeed(1.6); // used by Anitescu stepper only
mphysicalSystem.SetIterLCPmaxItersSpeed(40);
mphysicalSystem.SetIterLCPmaxItersStab(20); // unuseful for Anitescu, only Tasora uses this
mphysicalSystem.SetIterLCPwarmStarting(true);
mphysicalSystem.SetParallelThreadNumber(4);
//
// THE SOFT-REAL-TIME CYCLE
//
application.SetStepManage(true);
application.SetTimestep(0.02);
while(application.GetDevice()->run())
{
application.GetVideoDriver()->beginScene(true, true, SColor(255,140,161,192));
ChIrrTools::drawGrid(application.GetVideoDriver(), 5,5, 20,20,
ChCoordsys<>(ChVector<>(0,0.2,0),Q_from_AngAxis(CH_C_PI/2,VECT_X)), video::SColor(50,90,90,150),true);
application.DrawAll();
application.DoStep();
application.GetVideoDriver()->endScene();
}
// Remember this at the end of the program, if you started
// with DLL_CreateGlobals();
DLL_DeleteGlobals();
return 0;
}
void create_jengatower_bodies(ChSystem& mphysicalSystem)
{
// Create a material that will be shared between bricks
ChSharedPtr<ChMaterialSurface> mmaterial(new ChMaterialSurface);
mmaterial->SetFriction(0.4f);
mmaterial->SetCompliance (0.0000005f);
mmaterial->SetComplianceT(0.0000005f);
mmaterial->SetDampingF(0.2f);
// Create bricks
for (int bi = 0; bi < 12; bi+=2)
{
ChSharedPtr<ChBodyEasyBox> mrigidBody1(new ChBodyEasyBox(
2, 2, 14, // x,y,z size
100, // density
true, // collide enable?
true)); // visualization?
mrigidBody1->SetPos( ChVector<>(-5, 1.0+bi*2.0, 0) );
mrigidBody1->SetMaterialSurface(mmaterial); // use shared surface properties
mphysicalSystem.Add(mrigidBody1);
ChSharedPtr<ChBodyEasyBox> mrigidBody2(new ChBodyEasyBox(
2, 2, 14, // x,y,z size
100, // density
true, // collide enable?
true)); // visualization?
mrigidBody2->SetPos( ChVector<>( 5, 1.0+bi*2.0, 0) );
mrigidBody2->SetMaterialSurface(mmaterial); // use shared surface properties
mphysicalSystem.Add(mrigidBody2);
ChSharedPtr<ChBodyEasyBox> mrigidBody3(new ChBodyEasyBox(
14, 2, 2, // x,y,z size
100, // density
true, // collide enable?
true)); // visualization?
mrigidBody3->SetPos( ChVector<>(0, 3.0+bi*2.0, 5) );
mrigidBody3->SetMaterialSurface(mmaterial); // use shared surface properties
mphysicalSystem.Add(mrigidBody3);
ChSharedPtr<ChBodyEasyBox> mrigidBody4(new ChBodyEasyBox(
14, 2, 2, // x,y,z size
100, // density
true, // collide enable?
true)); // visualization?
mrigidBody4->SetPos( ChVector<>(0, 3.0+bi*2.0, -5) );
mrigidBody4->SetMaterialSurface(mmaterial); // use shared surface properties
mphysicalSystem.Add(mrigidBody4);
}
// Create the floor using
// fixed rigid body of 'box' type:
ChSharedPtr<ChBodyEasyBox> mrigidFloor(new ChBodyEasyBox(
250, 4, 250, // x,y,z size
1000, // density
true, // collide enable?
true)); // visualization?
mrigidFloor->SetPos(ChVector<>(0,-2,0));
mrigidFloor->SetMaterialSurface(mmaterial);
mrigidFloor->SetBodyFixed(true);
mphysicalSystem.Add(mrigidFloor);
// Create a ball that will collide with tower
ChSharedPtr<ChBodyEasySphere> mrigidBall(new ChBodyEasySphere(
4, // radius
1000, // density
true, // collide enable?
true)); // visualization?
mrigidBall->SetMaterialSurface(mmaterial);
mrigidBall->SetPos(ChVector<>(0, 3, -8));
mrigidBall->SetPos_dt(ChVector<>(0,0,2)); // set initial speed
mrigidBall->GetMaterialSurface()->SetFriction(0.4f); // use own (not shared) matrial properties
mrigidBall->GetMaterialSurface()->SetCompliance(0.0);
mrigidBall->GetMaterialSurface()->SetComplianceT(0.0);
mrigidBall->GetMaterialSurface()->SetDampingF(0.2f);
mphysicalSystem.Add(mrigidBall);
// optional, attach a texture for better visualization
ChSharedPtr<ChTexture> mtextureball(new ChTexture());
mtextureball->SetTextureFilename("../data/bluwhite.png");
mrigidBall->AddAsset(mtextureball);
}
// Delete the car object, deleting also all bodies corresponding to
// the various parts and removing them from the physical system. Also
// removes constraints from the system.
~MySimpleForklift() {
ChSystem* mysystem = truss->GetSystem(); // trick to get the system here
mysystem->Remove(link_revoluteRF);
mysystem->Remove(link_revoluteLF);
mysystem->Remove(link_steer_engineB);
mysystem->Remove(link_engineB);
mysystem->Remove(link_engineArm);
mysystem->Remove(link_prismaticFork);
mysystem->Remove(link_actuatorFork);
mysystem->Remove(truss);
mysystem->Remove(wheelRF);
mysystem->Remove(wheelLF);
mysystem->Remove(wheelB);
mysystem->Remove(spindleB);
mysystem->Remove(arm);
mysystem->Remove(fork);
}
int main(int argc, char* argv[]) {
DLL_CreateGlobals();
ChSystem * system = new ChSystem();
system->SetMaxiter(100);
system->SetIterLCPmaxItersSpeed(100);
system->SetTol(0);
system->SetTolSpeeds(0);
ChLcpSystemDescriptor *mdescriptor = new ChLcpSystemDescriptor();
ChContactContainer *mcontactcontainer = new ChContactContainer();
//ChCollisionSystemBulletGPU *mcollisionengine = new ChCollisionSystemBulletGPU();
ChCollisionSystemBullet *mcollisionengine = new ChCollisionSystemBullet();
system->ChangeLcpSystemDescriptor(mdescriptor);
system->ChangeContactContainer(mcontactcontainer);
system->ChangeCollisionSystem(mcollisionengine);
system->SetIntegrationType(ChSystem::INT_ANITESCU);
system->SetLcpSolverType(ChSystem::LCP_ITERATIVE_APGD);
system->SetStep(timestep);
system->Set_G_acc(ChVector<>(0, gravity, 0));
ChSharedBodyPtr floor(new ChBody);
ChSharedBodyPtr chassis(new ChBody);
ChSharedBodyPtr axle_F(new ChBody);
ChSharedBodyPtr axle_C(new ChBody);
ChSharedBodyPtr axle_R(new ChBody);
ChSharedBodyPtr leg_FR(new ChBody);
ChSharedBodyPtr leg_FL(new ChBody);
ChSharedBodyPtr leg_CR(new ChBody);
ChSharedBodyPtr leg_CL(new ChBody);
ChSharedBodyPtr leg_RR(new ChBody);
ChSharedBodyPtr leg_RL(new ChBody);
InitObject(floor, 1.0, ChVector<>(0, -3.5, 0), Quaternion(1, 0, 0, 0), 1, 1, 0, true, true, -20, -20);
InitObject(chassis, 1, ChVector<>(0, 0, 0), Quaternion(1, 0, 0, 0), 1, 1, 0, true, false, 0, 1);
InitObject(axle_F, 1, ChVector<>(0, 0, chassisL / 2), Q_from_AngZ(CH_C_PI / 2.0), 1, 1, 0, false, false, -2, -2);
InitObject(axle_C, 1, ChVector<>(0, 0, 0), Q_from_AngZ(CH_C_PI / 2.0), 1, 1, 0, false, false, -2, -2);
InitObject(axle_R, 1, ChVector<>(0, 0, -chassisL / 2), Q_from_AngZ(CH_C_PI / 2.0), 1, 1, 0, false, false, -2, -2);
InitObject(leg_FR, 1, ChVector<>((axleL + legW) / 2.0, -legL / 2.0, chassisL / 2), Quaternion(1, 0, 0, 0), 1, 1, 0, true, false, 2, 2);
InitObject(leg_FL, 1, ChVector<>(-(axleL + legW) / 2.0, legL / 2.0, chassisL / 2), Quaternion(1, 0, 0, 0), 1, 1, 0, true, false, 2, 2);
InitObject(leg_CR, 1, ChVector<>(-(axleL + legW) / 2.0, -legL / 2.0, 0), Quaternion(1, 0, 0, 0), 1, 1, 0, true, false, 2, 2);
InitObject(leg_CL, 1, ChVector<>((axleL + legW) / 2.0, legL / 2.0, 0), Quaternion(1, 0, 0, 0), 1, 1, 0, true, false, 2, 2);
InitObject(leg_RR, 1, ChVector<>((axleL + legW) / 2.0, -legL / 2.0, -chassisL / 2), Quaternion(1, 0, 0, 0), 1, 1, 0, true, false, 2, 2);
InitObject(leg_RL, 1, ChVector<>(-(axleL + legW) / 2.0, legL / 2.0, -chassisL / 2), Quaternion(1, 0, 0, 0), 1, 1, 0, true, false, 2, 2);
AddCollisionGeometry(floor, BOX, ChVector<>(10, 1 / 2.0, 10), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(chassis, ELLIPSOID, ChVector<>(1, 1, 1), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(axle_F, ELLIPSOID, ChVector<>(0.5 / 2.0, axleL / 2.0, 0.5 / 2.0), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(axle_C, ELLIPSOID, ChVector<>(0.5 / 2.0, axleL / 2.0, 0.5 / 2.0), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(axle_R, ELLIPSOID, ChVector<>(0.5 / 2.0, axleL / 2.0, 0.5 / 2.0), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(leg_FR, ELLIPSOID, ChVector<>(legW / 2.0, legL / 2.0, 0.5 / 2.0), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(leg_FL, ELLIPSOID, ChVector<>(legW / 2.0, legL / 2.0, 0.5 / 2.0), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(leg_CR, ELLIPSOID, ChVector<>(legW / 2.0, legL / 2.0, 0.5 / 2.0), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(leg_CL, ELLIPSOID, ChVector<>(legW / 2.0, legL / 2.0, 0.5 / 2.0), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(leg_RR, ELLIPSOID, ChVector<>(legW / 2.0, legL / 2.0, 0.5 / 2.0), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(leg_RL, ELLIPSOID, ChVector<>(legW / 2.0, legL / 2.0, 0.5 / 2.0), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
floor->SetInertiaXX(Vector(1,1,1));
chassis->SetInertiaXX(Vector(1,1,1));
axle_F->SetInertiaXX(Vector(1,1,1));
axle_C->SetInertiaXX(Vector(1,1,1));
axle_R->SetInertiaXX(Vector(1,1,1));
leg_FR->SetInertiaXX(Vector(1,1,1));
leg_FL->SetInertiaXX(Vector(1,1,1));
leg_CR->SetInertiaXX(Vector(1,1,1));
leg_CL->SetInertiaXX(Vector(1,1,1));
leg_RR->SetInertiaXX(Vector(1,1,1));
leg_RL->SetInertiaXX(Vector(1,1,1));
FinalizeObject(floor, (ChSystem *) system);
FinalizeObject(chassis, (ChSystem *) system);
FinalizeObject(axle_F, (ChSystem *) system);
FinalizeObject(axle_C, (ChSystem *) system);
FinalizeObject(axle_R, (ChSystem *) system);
FinalizeObject(leg_FR, (ChSystem *) system);
FinalizeObject(leg_FL, (ChSystem *) system);
FinalizeObject(leg_CR, (ChSystem *) system);
FinalizeObject(leg_CL, (ChSystem *) system);
FinalizeObject(leg_RR, (ChSystem *) system);
FinalizeObject(leg_RL, (ChSystem *) system);
ChSharedBodyPtr chassis_ptr = ChSharedBodyPtr(chassis);
ChSharedBodyPtr axle_F_ptr = ChSharedBodyPtr(axle_F);
ChSharedBodyPtr axle_C_ptr = ChSharedBodyPtr(axle_C);
ChSharedBodyPtr axle_R_ptr = ChSharedBodyPtr(axle_R);
ChSharedBodyPtr leg_FR_ptr = ChSharedBodyPtr(leg_FR);
ChSharedBodyPtr leg_FL_ptr = ChSharedBodyPtr(leg_FL);
ChSharedBodyPtr leg_CR_ptr = ChSharedBodyPtr(leg_CR);
ChSharedBodyPtr leg_CL_ptr = ChSharedBodyPtr(leg_CL);
ChSharedBodyPtr leg_RR_ptr = ChSharedBodyPtr(leg_RR);
ChSharedBodyPtr leg_RL_ptr = ChSharedBodyPtr(leg_RL);
// ChSharedPtr<ChLinkLockLock> axle_FR(new ChLinkLockLock);
// axle_FR->Initialize(leg_FR_ptr, axle_F_ptr, ChCoordsys<>(VNULL));
// system->AddLink(axle_FR);
////
// ChSharedPtr<ChLinkLockLock> axle_FL(new ChLinkLockLock);
// axle_FL->Initialize(leg_FL_ptr, axle_F_ptr, ChCoordsys<>(VNULL));
// system->AddLink(axle_FL);
//
// ChSharedPtr<ChLinkLockLock> axle_CR(new ChLinkLockLock);
// axle_CR->Initialize(leg_CR_ptr, axle_C_ptr, ChCoordsys<>(VNULL));
// system->AddLink(axle_CR);
//
// ChSharedPtr<ChLinkLockLock> axle_CL(new ChLinkLockLock);
// axle_CL->Initialize(leg_CL_ptr, axle_C_ptr, ChCoordsys<>(VNULL));
// system->AddLink(axle_CL);
//
// ChSharedPtr<ChLinkLockLock> axle_RR(new ChLinkLockLock);
// axle_RR->Initialize(leg_RR_ptr, axle_R_ptr, ChCoordsys<>(VNULL));
// system->AddLink(axle_RR);
//
// ChSharedPtr<ChLinkLockLock> axle_RL(new ChLinkLockLock);
// axle_RL->Initialize(leg_RL_ptr, axle_R_ptr, ChCoordsys<>(VNULL));
// system->AddLink(axle_RL);
// Create engine between axles and chassis
GetLog() << "Creating Motors\n";
// ChSharedPtr<ChLinkEngine> eng_F(new ChLinkEngine);
// eng_F->Initialize(axle_F_ptr, chassis_ptr, ChCoordsys<>(ChVector<>(0, 0, chassisL / 2), Q_from_AngY(CH_C_PI / 2)));
// eng_F->Set_shaft_mode(ChLinkEngine::ENG_SHAFT_LOCK); // also works as revolute support
// eng_F->Set_eng_mode(ChLinkEngine::ENG_MODE_SPEED);
// if (ChFunction_Const* mfun = dynamic_cast<ChFunction_Const*>(eng_F->Get_spe_funct()))
// mfun->Set_yconst(1); // rad/s angular speed
// system->AddLink(eng_F);
ChSharedPtr<ChLinkEngine> eng_C(new ChLinkEngine);
eng_C->Initialize(axle_C_ptr, chassis_ptr, ChCoordsys<>(ChVector<>(0, 0, 0), Q_from_AngY(CH_C_PI / 2)));
eng_C->Set_shaft_mode(ChLinkEngine::ENG_SHAFT_LOCK); // also works as revolute support
eng_C->Set_eng_mode(ChLinkEngine::ENG_MODE_SPEED);
if (ChFunction_Const* mfun = dynamic_cast<ChFunction_Const*>(eng_C->Get_spe_funct()))
mfun->Set_yconst(1); // rad/s angular speed
system->AddLink(eng_C);
// ChSharedPtr<ChLinkEngine> eng_R(new ChLinkEngine);
// eng_R->Initialize(axle_R_ptr, chassis_ptr, ChCoordsys<>(ChVector<>(0, 0, -chassisL / 2), Q_from_AngY(CH_C_PI / 2)));
// eng_R->Set_shaft_mode(ChLinkEngine::ENG_SHAFT_LOCK); // also works as revolute support
// eng_R->Set_eng_mode(ChLinkEngine::ENG_MODE_SPEED);
// if (ChFunction_Const* mfun = dynamic_cast<ChFunction_Const*>(eng_R->Get_spe_funct()))
// mfun->Set_yconst(1); // rad/s angular speed
// system->AddLink(eng_R);
system->DoStepDynamics(timestep);
system->DoStepDynamics(timestep);
exit(0);
ChOpenGLManager * window_manager = new ChOpenGLManager();
ChOpenGL openGLView(window_manager, system, 800, 600, 0, 0, "Test_Solvers");
openGLView.render_camera->camera_position = glm::vec3(0, -5, -10);
openGLView.render_camera->camera_look_at = glm::vec3(0, -5, 0);
openGLView.render_camera->camera_scale = .5;
openGLView.SetCustomCallback(RunTimeStep);
openGLView.StartSpinning(window_manager);
window_manager->CallGlutMainLoop();
DLL_DeleteGlobals();
return 0;
}
// Build and initialize the tank, creating all bodies corresponding to
// the various parts and adding them to the physical system - also creating
// and adding constraints to the system.
MySimpleTank(ChSystem& my_system, ///< the chrono::engine physical system
ISceneManager* msceneManager, ///< the Irrlicht scene manager for 3d shapes
IVideoDriver* mdriver ///< the Irrlicht video driver
)
{
throttleL = throttleR = 0; // initially, gas throttle is 0.
max_motor_speed = 10;
double my = 0.5; // left back hub pos
double mx = 0;
double shoelength = 0.2;
double shoethickness = 0.06;
double shoewidth = 0.3;
double shoemass = 2;
double radiustrack = 0.31;
double wheeldiameter = 0.280*2;
int nwrap = 6;
int ntiles = 7;
double rlwidth = 1.20;
double passo = (ntiles+1)*shoelength;
ChVector<> cyl_displA(0, 0.075+0.02,0);
ChVector<> cyl_displB(0, -0.075-0.02,0);
double cyl_hthickness = 0.045;
// --- The tank body ---
IAnimatedMesh* bulldozer_bodyMesh = msceneManager->getMesh("../data/bulldozerB10.obj");
truss = (ChBodySceneNode*)addChBodySceneNode(
&my_system, msceneManager, bulldozer_bodyMesh,
350.0,
ChVector<>(mx + passo/2, my + radiustrack , rlwidth/2),
QUNIT);
truss->GetBody()->SetInertiaXX(ChVector<>(13.8, 13.5, 10));
truss->GetBody()->SetBodyFixed(false);
//truss->addShadowVolumeSceneNode();
// --- Right Front suspension ---
// Load a triangle mesh for wheel visualization
IAnimatedMesh* irmesh_wheel_view = msceneManager->getMesh("../data/wheel_view.obj");
// ..the tank right-front wheel
wheelRF = (ChBodySceneNode*) addChBodySceneNode(
&my_system, msceneManager, irmesh_wheel_view,
9.0,
ChVector<>(mx + passo, my+ radiustrack , 0),
chrono::Q_from_AngAxis(CH_C_PI/2, VECT_X) );
wheelRF->GetBody()->GetCollisionModel()->ClearModel();
wheelRF->GetBody()->GetCollisionModel()->AddCylinder(wheeldiameter/2,wheeldiameter/2, cyl_hthickness, &cyl_displA);
wheelRF->GetBody()->GetCollisionModel()->AddCylinder(wheeldiameter/2,wheeldiameter/2, cyl_hthickness, &cyl_displB);
wheelRF->GetBody()->GetCollisionModel()->BuildModel(); // Creates the collision model
wheelRF->GetBody()->SetCollide(true);
wheelRF->GetBody()->SetInertiaXX(ChVector<>(1.2, 1.2, 1.2));
wheelRF->GetBody()->SetFriction(1.0);
video::ITexture* cylinderMap = mdriver->getTexture("../data/bluwhite.png");
wheelRF->setMaterialTexture(0, cylinderMap);
wheelRF->addShadowVolumeSceneNode();
// .. create the revolute joint between the wheel and the truss
link_revoluteRF = ChSharedPtr<ChLinkLockRevolute>(new ChLinkLockRevolute); // right, front, upper, 1
link_revoluteRF->Initialize(wheelRF->GetBody(), truss->GetBody(),
ChCoordsys<>(ChVector<>(mx + passo, my+ radiustrack , 0) , QUNIT ) );
my_system.AddLink(link_revoluteRF);
// --- Left Front suspension ---
// ..the tank left-front wheel
wheelLF = (ChBodySceneNode*) addChBodySceneNode(
&my_system, msceneManager, irmesh_wheel_view,
9.0,
ChVector<>(mx + passo, my+ radiustrack , rlwidth),
chrono::Q_from_AngAxis(CH_C_PI/2, VECT_X) );
wheelLF->GetBody()->GetCollisionModel()->ClearModel();
wheelLF->GetBody()->GetCollisionModel()->AddCylinder(wheeldiameter/2,wheeldiameter/2, cyl_hthickness, &cyl_displA);
wheelLF->GetBody()->GetCollisionModel()->AddCylinder(wheeldiameter/2,wheeldiameter/2, cyl_hthickness, &cyl_displB);
wheelLF->GetBody()->GetCollisionModel()->BuildModel(); // Creates the collision model
wheelLF->GetBody()->SetCollide(true);
wheelLF->GetBody()->SetInertiaXX(ChVector<>(1.2, 1.2, 1.2));
wheelLF->GetBody()->SetFriction(1.0);
wheelLF->setMaterialTexture(0, cylinderMap);
wheelLF->addShadowVolumeSceneNode();
// .. create the revolute joint between the wheel and the truss
link_revoluteLF = ChSharedPtr<ChLinkLockRevolute>(new ChLinkLockRevolute); // left, front, upper, 1
link_revoluteLF->Initialize(wheelLF->GetBody(), truss->GetBody(),
ChCoordsys<>(ChVector<>(mx + passo, my+ radiustrack , rlwidth) , QUNIT ) );
my_system.AddLink(link_revoluteLF);
// --- Right Back suspension ---
// ..the tank right-back wheel
wheelRB = (ChBodySceneNode*) addChBodySceneNode(
&my_system, msceneManager, irmesh_wheel_view,
9.0,
ChVector<>(mx , my+ radiustrack , 0),
chrono::Q_from_AngAxis(CH_C_PI/2, VECT_X) );
wheelRB->GetBody()->GetCollisionModel()->ClearModel();
wheelRB->GetBody()->GetCollisionModel()->AddCylinder(wheeldiameter/2,wheeldiameter/2, cyl_hthickness, &cyl_displA);
wheelRB->GetBody()->GetCollisionModel()->AddCylinder(wheeldiameter/2,wheeldiameter/2, cyl_hthickness, &cyl_displB);
wheelRB->GetBody()->GetCollisionModel()->BuildModel(); // Creates the collision model
wheelRB->GetBody()->SetCollide(true);
wheelRB->GetBody()->SetInertiaXX(ChVector<>(1.2, 1.2, 1.2));
wheelRB->GetBody()->SetFriction(1.0);
cylinderMap = mdriver->getTexture("../data/bluwhite.png");
wheelRB->setMaterialTexture(0, cylinderMap);
wheelRB->addShadowVolumeSceneNode();
// .. create the motor joint between the wheel and the truss (simplified motor model: just impose speed..)
link_revoluteRB = ChSharedPtr<ChLinkEngine>(new ChLinkEngine); // right, back, upper, 1
link_revoluteRB->Initialize(wheelRB->GetBody(), truss->GetBody(),
ChCoordsys<>(ChVector<>(mx , my+ radiustrack , 0) , QUNIT ) );
link_revoluteRB->Set_eng_mode(ChLinkEngine::ENG_MODE_SPEED);
my_system.AddLink(link_revoluteRB);
// --- Left Back suspension ---
// ..the tank left-back wheel
wheelLB = (ChBodySceneNode*) addChBodySceneNode(
&my_system, msceneManager, irmesh_wheel_view,
9.0,
ChVector<>(mx , my+ radiustrack , rlwidth),
chrono::Q_from_AngAxis(CH_C_PI/2, VECT_X) );
wheelLB->GetBody()->GetCollisionModel()->ClearModel();
wheelLB->GetBody()->GetCollisionModel()->AddCylinder(wheeldiameter/2,wheeldiameter/2, cyl_hthickness, &cyl_displA);
wheelLB->GetBody()->GetCollisionModel()->AddCylinder(wheeldiameter/2,wheeldiameter/2, cyl_hthickness, &cyl_displB);
wheelLB->GetBody()->GetCollisionModel()->BuildModel(); // Creates the collision model
wheelLB->GetBody()->SetCollide(true);
wheelLB->GetBody()->SetInertiaXX(ChVector<>(1.2, 1.2, 1.2));
wheelLB->GetBody()->SetFriction(1.0);
wheelLB->setMaterialTexture(0, cylinderMap);
wheelLB->addShadowVolumeSceneNode();
// .. create the motor joint between the wheel and the truss (simplified motor model: just impose speed..)
link_revoluteLB = ChSharedPtr<ChLinkEngine>(new ChLinkEngine); // left, back, upper, 1
link_revoluteLB->Initialize(wheelLB->GetBody(), truss->GetBody(),
ChCoordsys<>(ChVector<>(mx , my+ radiustrack , rlwidth) , QUNIT ) );
link_revoluteLB->Set_eng_mode(ChLinkEngine::ENG_MODE_SPEED);
my_system.AddLink(link_revoluteLB);
//--- TRACKS ---
// Load a triangle mesh for visualization
IAnimatedMesh* irmesh_shoe_view = msceneManager->getMesh("../data/shoe_view.obj");
// Load a triangle mesh for collision
IAnimatedMesh* irmesh_shoe_collision = msceneManager->getMesh("../data/shoe_collision.obj");
ChTriangleMeshSoup temp_trianglemesh;
fillChTrimeshFromIrlichtMesh(&temp_trianglemesh, irmesh_shoe_collision->getMesh(0));
chrono::ChVector<> mesh_displacement(shoelength*0.5,0,0); // since mesh origin is not in body center of mass
chrono::ChVector<> joint_displacement(-shoelength*0.5,0,0); // position of shoe-shoe constraint, relative to COG.
chrono::ChVector<> position;
chrono::ChQuaternion<> rotation;
for (int side = 0; side <2; side ++)
{
mx = 0;
mx += shoelength;
double mz = 0;
if (side == 0)
mz = 0;
else
mz = rlwidth;
position.Set(mx, my, mz);
rotation = QUNIT;
// Create sample body (with empty collision shape; later create the collision model by adding the coll.shapes)
ChBodySceneNode* firstBodyShoe = (ChBodySceneNode*)addChBodySceneNode(
&my_system, msceneManager, 0,
shoemass,
position,
rotation);
// Creates the collision model with a (simplified) mesh
ChVector<> pin_displacement = mesh_displacement + ChVector<>(0,0.05,0);
firstBodyShoe->GetBody()->GetCollisionModel()->SetSafeMargin(0.004); // inward safe margin
firstBodyShoe->GetBody()->GetCollisionModel()->SetEnvelope(0.010); // distance of the outward "collision envelope"
firstBodyShoe->GetBody()->GetCollisionModel()->ClearModel();
// ...try to use a concave (simplified) mesh plus automatic convex decomposition?? ...
firstBodyShoe->GetBody()->GetCollisionModel()->AddTriangleMesh(temp_trianglemesh, false, false, &mesh_displacement); // not static, not convex
// .. or rather use few 'primitive' shapes to approximate with cubes/etc??
//firstBodyShoe->GetBody()->GetCollisionModel()->AddBox(shoelength/2, shoethickness/2, shoewidth/2, &mesh_displacement);
//firstBodyShoe->GetBody()->GetCollisionModel()->AddBox(0.04, 0.02, 0.02, &pin_displacement);
firstBodyShoe->GetBody()->GetCollisionModel()->BuildModel(); // Creates the collision model
firstBodyShoe->GetBody()->SetCollide(true);
/* // alternative: use box as a collision geometry (but wheels will slip..)
ChBodySceneNode* firstBodyShoe = (ChBodySceneNode*)addChBodySceneNode_easyBox(
&my_system, msceneManager,
shoemass,
position,
rotation,
ChVector<>(shoelength, 0.02, 0.2) );
*/
// Add a mesh for visualization purposes
msceneManager->addAnimatedMeshSceneNode(irmesh_shoe_view, firstBodyShoe,-1, vector3dfCH(mesh_displacement));
// Avoid creation of contacts with neighbouring shoes, using
// a collision family (=3) that does not collide with itself
firstBodyShoe->GetBody()->GetCollisionModel()->SetFamily(3);
firstBodyShoe->GetBody()->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(3);
// Other settings
firstBodyShoe->GetBody()->SetInertiaXX(ChVector<>(0.1, 0.1, 0.1));
ChBodySceneNode* previous_rigidBodyShoe = firstBodyShoe;
for (int nshoe = 1; nshoe < ntiles; nshoe++)
{
mx += shoelength;
position.Set(mx, my, mz);
ChBodySceneNode* rigidBodyShoe = MakeShoe(previous_rigidBodyShoe,
firstBodyShoe,
position, rotation,
irmesh_shoe_view,
my_system, msceneManager, mdriver,
mesh_displacement, joint_displacement, shoemass);
previous_rigidBodyShoe = rigidBodyShoe;
}
for (int nshoe = 0; nshoe < nwrap; nshoe++)
{
double alpha = (CH_C_PI/((double)(nwrap-1.0)))*((double)nshoe);
double lx = mx + shoelength + radiustrack * sin(alpha);
double ly = my + radiustrack - radiustrack * cos(alpha);
position.Set(lx, ly, mz);
rotation = chrono::Q_from_AngAxis(alpha,ChVector<>(0,0,1));
ChBodySceneNode* rigidBodyShoe = MakeShoe(previous_rigidBodyShoe,
firstBodyShoe,
position, rotation,
irmesh_shoe_view,
my_system, msceneManager, mdriver,
mesh_displacement, joint_displacement, shoemass);
previous_rigidBodyShoe = rigidBodyShoe;
}
for (int nshoe = (ntiles-1); nshoe >= 0; nshoe--)
{
position.Set(mx, my+2*radiustrack, mz);
ChBodySceneNode* rigidBodyShoe = MakeShoe(previous_rigidBodyShoe,
firstBodyShoe,
position, rotation,
irmesh_shoe_view,
my_system, msceneManager, mdriver,
mesh_displacement, joint_displacement, shoemass);
previous_rigidBodyShoe = rigidBodyShoe;
mx -= shoelength;
}
for (int nshoe = 0; nshoe < nwrap; nshoe++)
{
double alpha = CH_C_PI + (CH_C_PI/((double)(nwrap-1.0)))*((double)nshoe);
double lx = mx + 0 + radiustrack * sin(alpha);
double ly = my + radiustrack - radiustrack * cos(alpha);
position.Set(lx, ly, mz);
rotation = chrono::Q_from_AngAxis(alpha,ChVector<>(0,0,1));
ChBodySceneNode* rigidBodyShoe = MakeShoe(previous_rigidBodyShoe,
firstBodyShoe,
position, rotation,
irmesh_shoe_view,
my_system, msceneManager, mdriver,
mesh_displacement, joint_displacement, shoemass);
previous_rigidBodyShoe = rigidBodyShoe;
}
// close track
ChVector<> linkpos = firstBodyShoe->GetBody()->Point_Body2World(&joint_displacement);
ChSharedPtr<ChLinkLockRevolute> link_revolute_shoeshoe(new ChLinkLockRevolute);
link_revolute_shoeshoe->Initialize(firstBodyShoe->GetBody(), previous_rigidBodyShoe->GetBody(),
ChCoordsys<>( linkpos , QUNIT) );
my_system.AddLink(link_revolute_shoeshoe);
}
}
int main(int argc, char* argv[])
{
// In CHRONO engine, The DLL_CreateGlobals() - DLL_DeleteGlobals(); pair is needed if
// global functions are needed.
DLL_CreateGlobals();
// Create a ChronoENGINE physical system
ChSystem mphysicalSystem;
// Create the Irrlicht visualization (open the Irrlicht device,
// bind a simple user interface, etc. etc.)
ChIrrAppInterface application(&mphysicalSystem, L"Benchmark application",core::dimension2d<u32>(800,600),false);
// Easy shortcuts to add logo, camera, lights and sky in Irrlicht scene:
ChIrrWizard::add_typical_Lights(application.GetDevice());
ChIrrWizard::add_typical_Camera(application.GetDevice(), core::vector3df(0,14,-40));
//
// CREATE THE MECHANICAL SYSTEM OF CHRONO...
//
application.GetVideoDriver()->beginScene(true, true, SColor(255,140,161,192));
application.GetVideoDriver()->endScene();
GetLog() << " ** PLEASE WAIT FOR INITIALIZATION! \n ** This may take seconds or minutes..\n\n";
// Prepare the physical system for the simulation
mphysicalSystem.SetIntegrationType(ChSystem::INT_ANITESCU);
//mphysicalSystem.SetIntegrationType(ChSystem::INT_TASORA);
//mphysicalSystem.SetLcpSolverType(ChSystem::LCP_ITERATIVE_SYMMSOR); // here would be the better choiche since the wall is build ground up.
//mphysicalSystem.SetLcpSolverType(ChSystem::LCP_ITERATIVE_SOR);
//mphysicalSystem.SetLcpSolverType(ChSystem::LCP_ITERATIVE_JACOBI);
//mphysicalSystem.SetLcpSolverType(ChSystem::LCP_ITERATIVE_SOR_MULTITHREAD);
//mphysicalSystem.SetParallelThreadNumber(4);
mphysicalSystem.SetMaxPenetrationRecoverySpeed(1.6); // used by Anitescu stepper only
mphysicalSystem.SetIterLCPmaxItersSpeed(20);
mphysicalSystem.SetIterLCPmaxItersStab(10); // unuseful for Anitescu, only Tasora uses this
//mphysicalSystem.SetIterLCPwarmStarting(true);
// Test convergence for varying omega
//
// (solver:SOR)
//
if (false)
{
ChStreamOutAsciiFile data_err_SOR_Niter("data_err_SOR_Niter.dat");
ChStreamOutAsciiFile data_dgamma_SOR_Niter("data_dgamma_SOR_Niter.dat");
int plotframe = 500;
int ntestspheres = 220;
mphysicalSystem.SetLcpSolverType(ChSystem::LCP_ITERATIVE_SOR);
mphysicalSystem.SetIterLCPmaxItersSpeed(80);
mphysicalSystem.SetMaxPenetrationRecoverySpeed(2.0);
ChLcpIterativeSolver* msolver = (ChLcpIterativeSolver*)mphysicalSystem.GetLcpSolverSpeed();
msolver->SetRecordViolation(true);
for (double momega = 0.2; (momega <= 1.401 && application.GetDevice()->run()); momega +=0.2)
{
ISceneNode* parent = application.GetSceneManager()->addEmptySceneNode();
mphysicalSystem.SetIterLCPomega(0.8);
mphysicalSystem.SetIterLCPsharpnessLambda(1.0);
mphysicalSystem.SetMaxPenetrationRecoverySpeed(2.0);
// create spheres and walls, and insert into that scene node
ChSetRandomSeed(123);
create_some_falling_items(mphysicalSystem, application.GetSceneManager(), application.GetVideoDriver(), parent, ntestspheres, 1.6, 10);
int totframes = 0;
// ------- begin simulation loop -----
while(application.GetDevice()->run())
{
totframes++;
application.GetVideoDriver()->beginScene(true, true, SColor(255,140,161,192));
application.DrawAll();
if (totframes==plotframe)
{
mphysicalSystem.SetIterLCPomega(momega);
mphysicalSystem.SetIterLCPsharpnessLambda(0.7);
mphysicalSystem.SetMaxPenetrationRecoverySpeed(0.0);
}
// integrate
mphysicalSystem.DoStepDynamics(0.01);
application.GetVideoDriver()->endScene();
//***PLOT***
if (totframes==plotframe)
{
GetLog()<< "Saving data\n";
ChLcpIterativeSolver* msolver = (ChLcpIterativeSolver*)mphysicalSystem.GetLcpSolverSpeed();
for (int k=0; k< msolver->GetViolationHistory().size(); k++)
{
data_err_SOR_Niter << msolver->GetViolationHistory()[k] << " ";
}
for (int j=0; j< msolver->GetDeltalambdaHistory().size(); j++)
{
data_dgamma_SOR_Niter << msolver->GetDeltalambdaHistory()[j] << " ";
}
}
if(totframes>plotframe) break;
} // ------- end of simulation loop -----
GetLog()<< " num contacts " << mphysicalSystem.GetNcontacts() << "\n";
data_err_SOR_Niter << "\n";
data_dgamma_SOR_Niter << "\n";
// remove the root scene node (contains walls & spheres but no camera no lights)
// to reset and prepare for new scene at next for() loop.
parent->remove();
// Now the system should already have no bodies, because of full removal from Irrlicht scene.
// Then, the following clearing should be unuseful...(do it anyway..)
mphysicalSystem.Clear();
}
} // end test
// Test convergence for varying omega
//
// (solver:JACOBI)
//
if (false)
{
ChStreamOutAsciiFile data_err_JACOBI_Niter("data_err_JACOBI_Niter_L10_at500_ok.dat");
ChStreamOutAsciiFile data_dgamma_JACOBI_Niter("data_dgamma_JACOBI_Niter_L10_at500_ok.dat");
int plotframe = 500;
int ntestspheres = 220;
mphysicalSystem.SetLcpSolverType(ChSystem::LCP_ITERATIVE_SOR);
mphysicalSystem.SetIterLCPomega(0.8);
mphysicalSystem.SetIterLCPmaxItersSpeed(80);
mphysicalSystem.SetMaxPenetrationRecoverySpeed(2.0);
ChLcpIterativeSolver* msolver = (ChLcpIterativeSolver*)mphysicalSystem.GetLcpSolverSpeed();
msolver->SetRecordViolation(true);
for (double momega = 0.1; (momega <= 0.71 && application.GetDevice()->run()); momega +=0.1)
{
ISceneNode* parent = application.GetSceneManager()->addEmptySceneNode();
mphysicalSystem.SetLcpSolverType(ChSystem::LCP_ITERATIVE_SOR);
mphysicalSystem.SetIterLCPomega(0.8);
mphysicalSystem.SetIterLCPmaxItersSpeed(80);
mphysicalSystem.SetMaxPenetrationRecoverySpeed(2.0);
mphysicalSystem.SetIterLCPsharpnessLambda(1.0);
// create spheres and walls, and insert into that scene node
ChSetRandomSeed(123);
create_some_falling_items(mphysicalSystem, application.GetSceneManager(), application.GetVideoDriver(), parent, ntestspheres, 1.6, 10);
int totframes = 0;
// ------- begin simulation loop -----
while(application.GetDevice()->run())
{
totframes++;
application.GetVideoDriver()->beginScene(true, true, SColor(255,140,161,192));
application.DrawAll();
if (totframes==plotframe)
{
mphysicalSystem.SetLcpSolverType(ChSystem::LCP_ITERATIVE_JACOBI);
ChLcpIterativeSolver* msolver = (ChLcpIterativeSolver*)mphysicalSystem.GetLcpSolverSpeed();
msolver->SetRecordViolation(true);
mphysicalSystem.SetIterLCPomega(momega);
mphysicalSystem.SetIterLCPsharpnessLambda(1.0);
mphysicalSystem.SetMaxPenetrationRecoverySpeed(0.0);
}
// integrate
mphysicalSystem.DoStepDynamics(0.01);
application.GetVideoDriver()->endScene();
//***PLOT***
if (totframes==plotframe)
{
GetLog()<< "Saving data\n";
ChLcpIterativeSolver* msolver = (ChLcpIterativeSolver*)mphysicalSystem.GetLcpSolverSpeed();
for (int k=0; k< msolver->GetViolationHistory().size(); k++)
{
data_err_JACOBI_Niter << msolver->GetViolationHistory()[k] << " ";
}
for (int j=0; j< msolver->GetDeltalambdaHistory().size(); j++)
{
data_dgamma_JACOBI_Niter << msolver->GetDeltalambdaHistory()[j] << " ";
}
}
if(totframes>plotframe) break;
} // ------- end of simulation loop -----
GetLog()<< " num contacts " << mphysicalSystem.GetNcontacts() << "\n";
data_err_JACOBI_Niter << "\n";
data_dgamma_JACOBI_Niter << "\n";
// remove the root scene node (contains walls & spheres but no camera no lights)
// to reset and prepare for new scene at next for() loop.
parent->remove();
// Now the system should already have no bodies, because of full removal from Irrlicht scene.
// Then, the following clearing should be unuseful...(do it anyway..)
mphysicalSystem.Clear();
}
} // end test
// Test lambda effect
//
// Velocity error vs. Num. velocity iterations vs. Lambda
// (solver:SOR)
//
if (false)
{
ChStreamOutAsciiFile data_err_SOR_Niter("data_err_SOR_Niter_om1.dat");
ChStreamOutAsciiFile data_dgamma_SOR_Niter("data_dgamma_SOR_Niter_om1.dat");
int plotframe = 500;
int ntestspheres = 220;
mphysicalSystem.SetLcpSolverType(ChSystem::LCP_ITERATIVE_SOR);
mphysicalSystem.SetIterLCPmaxItersSpeed(80);
mphysicalSystem.SetMaxPenetrationRecoverySpeed(2.0);
ChLcpIterativeSolver* msolver = (ChLcpIterativeSolver*)mphysicalSystem.GetLcpSolverSpeed();
msolver->SetRecordViolation(true);
for (double mlambda = 0.2; (mlambda <= 1.01 && application.GetDevice()->run()); mlambda +=0.2)
{
ISceneNode* parent = application.GetSceneManager()->addEmptySceneNode();
mphysicalSystem.SetIterLCPomega(0.8);
mphysicalSystem.SetIterLCPsharpnessLambda(1.0);
mphysicalSystem.SetMaxPenetrationRecoverySpeed(2.0);
// create spheres and walls, and insert into that scene node
ChSetRandomSeed(123);
create_some_falling_items(mphysicalSystem, application.GetSceneManager(), application.GetVideoDriver(), parent, ntestspheres, 1.6, 10);
int totframes = 0;
// ------- begin simulation loop -----
while(application.GetDevice()->run())
{
totframes++;
application.GetVideoDriver()->beginScene(true, true, SColor(255,140,161,192));
application.DrawAll();
if (totframes==plotframe)
{
mphysicalSystem.SetIterLCPomega(1.0);
mphysicalSystem.SetIterLCPsharpnessLambda(mlambda);
mphysicalSystem.SetMaxPenetrationRecoverySpeed(0.0);
}
// integrate
mphysicalSystem.DoStepDynamics(0.01);
application.GetVideoDriver()->endScene();
//***PLOT***
if (totframes==plotframe)
{
GetLog()<< "Saving data\n";
ChLcpIterativeSolver* msolver = (ChLcpIterativeSolver*)mphysicalSystem.GetLcpSolverSpeed();
for (int k=0; k< msolver->GetViolationHistory().size(); k++)
{
data_err_SOR_Niter << msolver->GetViolationHistory()[k] << " ";
}
for (int j=0; j< msolver->GetDeltalambdaHistory().size(); j++)
{
data_dgamma_SOR_Niter << msolver->GetDeltalambdaHistory()[j] << " ";
}
}
if(totframes>plotframe) break;
} // ------- end of simulation loop -----
GetLog()<< " num contacts " << mphysicalSystem.GetNcontacts() << "\n";
data_err_SOR_Niter << "\n";
data_dgamma_SOR_Niter << "\n";
// remove the root scene node (contains walls & spheres but no camera no lights)
// to reset and prepare for new scene at next for() loop.
parent->remove();
// Now the system should already have no bodies, because of full removal from Irrlicht scene.
// Then, the following clearing should be unuseful...(do it anyway..)
mphysicalSystem.Clear();
}
} // end test
// Test stack aspect ratio
//
//
if (false)
{
ChStreamOutAsciiFile data_err_SOR_Niter("data_err_aspectratio3.dat");
int plotframe = 500;
int ntestspheres = 220;
mphysicalSystem.SetLcpSolverType(ChSystem::LCP_ITERATIVE_SOR);
mphysicalSystem.SetIterLCPmaxItersSpeed(30);
mphysicalSystem.SetMaxPenetrationRecoverySpeed(2.0);
ChLcpIterativeSolver* msolver = (ChLcpIterativeSolver*)mphysicalSystem.GetLcpSolverSpeed();
msolver->SetRecordViolation(true);
for (double mspheres=10; (mspheres <= 330 && application.GetDevice()->run()); mspheres +=40)
{
for (int nviters = 20; (nviters <= 101 && application.GetDevice()->run()); nviters +=20)
{
ISceneNode* parent = application.GetSceneManager()->addEmptySceneNode();
mphysicalSystem.SetIterLCPomega(0.9);
mphysicalSystem.SetIterLCPsharpnessLambda(1.0);
mphysicalSystem.SetIterLCPmaxItersSpeed(30);
mphysicalSystem.SetMaxPenetrationRecoverySpeed(2.0);
// create spheres and walls, and insert into that scene node
ChSetRandomSeed(13);
create_some_falling_items(mphysicalSystem, application.GetSceneManager(), application.GetVideoDriver(), parent, mspheres, 1.6, 10,20.1);
int totframes = 0;
// ------- begin simulation loop -----
while(application.GetDevice()->run())
{
totframes++;
application.GetVideoDriver()->beginScene(true, true, SColor(255,140,161,192));
application.DrawAll();
if (totframes==plotframe)
{
mphysicalSystem.SetIterLCPomega(0.9);
mphysicalSystem.SetIterLCPmaxItersSpeed(nviters);
mphysicalSystem.SetMaxPenetrationRecoverySpeed(0.0);
}
application.GetVideoDriver()->beginScene(true, true, SColor(255,140,161,192));
application.DrawAll();
// integrate
mphysicalSystem.DoStepDynamics(0.01);
application.GetVideoDriver()->endScene();
//***PLOT***
if (totframes==plotframe)
{
GetLog()<< "Saving data\n";
ChLcpIterativeSolver* msolver = (ChLcpIterativeSolver*)mphysicalSystem.GetLcpSolverSpeed();
data_err_SOR_Niter << msolver->GetViolationHistory()[msolver->GetViolationHistory().size()-1] << " ";
}
if(totframes>plotframe) break;
} // ------- end of simulation loop -----
// remove the root scene node (contains walls & spheres but no camera no lights)
// to reset and prepare for new scene at next for() loop.
parent->remove();
// Now the system should already have no bodies, because of full removal from Irrlicht scene.
// Then, the following clearing should be unuseful...(do it anyway..)
mphysicalSystem.Clear();
}
data_err_SOR_Niter << "\n";
}
} // end test
// Test pos error
//
//
if (false)
{
ChStreamOutAsciiFile data_err_pos("data_err_pos.dat");
int plotframe = 500;
int ntestspheres = 120;
double mystep= 0;
for (double mdt=0.01; (mdt>0.00015 && application.GetDevice()->run()); mdt*=0.5)
{
GetLog() << "\n\nDT = " << mdt << "\n";
for (int nviters = 20; (nviters <= 81 && application.GetDevice()->run()); nviters +=20)
{
GetLog() << " nviters = " << nviters << "\n";
ISceneNode* parent = application.GetSceneManager()->addEmptySceneNode();
mystep = 0.01;
mphysicalSystem.SetChTime(0.0);
mphysicalSystem.SetIterLCPsharpnessLambda(0.8);
mphysicalSystem.SetIterLCPomega(0.8);
mphysicalSystem.SetLcpSolverType(ChSystem::LCP_ITERATIVE_SOR);
mphysicalSystem.SetIterLCPmaxItersSpeed(30);
mphysicalSystem.SetMaxPenetrationRecoverySpeed(2.0);
// create spheres and walls, and insert into that scene node
ChSetRandomSeed(13);
create_some_falling_items(mphysicalSystem, application.GetSceneManager(), application.GetVideoDriver(), parent, ntestspheres, 1.6, 10);
int totframes = 0;
int averagecnt = 0;
double average = 0;
// ------- begin simulation loop -----
while(application.GetDevice()->run())
{
totframes++;
application.GetVideoDriver()->beginScene(true, true, SColor(255,140,161,192));
application.DrawAll();
// start mode
if (mphysicalSystem.GetChTime()>2.4)
{
mphysicalSystem.SetIterLCPmaxItersSpeed(nviters);
mystep=mdt;
}
//start average
if (mphysicalSystem.GetChTime()>2.6)
{
class _label_reporter_class : public chrono::ChReportContactCallback
{
public:
virtual bool ReportContactCallback (
const chrono::ChVector<>& pA,
const chrono::ChVector<>& pB,
const chrono::ChMatrix33<>& plane_coord,
const double& distance,
const float& mfriction,
const chrono::ChVector<>& react_forces,
const chrono::ChVector<>& react_torques,
chrono::collision::ChCollisionModel* modA,
chrono::collision::ChCollisionModel* modB)
{
if (distance<maxpen)
maxpen = distance;
return true; // to continue scanning contacts
}
// Data
double maxpen;
};
_label_reporter_class my_label_rep;
my_label_rep.maxpen = 0;
// scan all contacts
mphysicalSystem.GetContactContainer()->ReportAllContacts(&my_label_rep);
//GetLog() << " p: " << my_label_rep.maxpen << "\n";
averagecnt++;
average += my_label_rep.maxpen;
}
// stop average & loop
if(mphysicalSystem.GetChTime()>2.8)
{
average /= (double)averagecnt;
GetLog() << " avg.err: " << average << "\n";
data_err_pos << average << " ";
break;
}
// integrate
mphysicalSystem.DoStepDynamics(mystep);
application.GetVideoDriver()->endScene();
} // ------- end of simulation loop -----
// remove the root scene node (contains walls & spheres but no camera no lights)
// to reset and prepare for new scene at next for() loop.
parent->remove();
// Now the system should already have no bodies, because of full removal from Irrlicht scene.
// Then, the following clearing should be unuseful...(do it anyway..)
mphysicalSystem.Clear();
}
data_err_pos << "\n";
}
} // end test
// Test pos error with Tasora stabilization
//
//
if (true)
{
ChStreamOutAsciiFile data_err_pos("data_err_tasora.dat");
int ntestspheres = 120;
for (int npiters = 20; (npiters <= 81 && application.GetDevice()->run()); npiters +=20)
{
GetLog() << " npiters = " << npiters << "\n";
for (int nviters = 20; (nviters <= 81 && application.GetDevice()->run()); nviters +=20)
{
GetLog() << " nviters = " << nviters << "\n";
ISceneNode* parent = application.GetSceneManager()->addEmptySceneNode();
mphysicalSystem.SetChTime(0.0);
mphysicalSystem.SetIterLCPsharpnessLambda(0.8);
mphysicalSystem.SetIterLCPomega(0.8);
mphysicalSystem.SetLcpSolverType(ChSystem::LCP_ITERATIVE_SOR);
mphysicalSystem.SetIntegrationType(ChSystem::INT_TASORA);
mphysicalSystem.SetIterLCPmaxItersSpeed(30);
mphysicalSystem.SetIterLCPmaxItersStab(30);
mphysicalSystem.SetMaxPenetrationRecoverySpeed(0.0);
// create spheres and walls, and insert into that scene node
ChSetRandomSeed(13);
create_some_falling_items(mphysicalSystem, application.GetSceneManager(), application.GetVideoDriver(), parent, ntestspheres, 1.6, 10);
int totframes = 0;
int averagecnt = 0;
double average = 0;
// ------- begin simulation loop -----
while(application.GetDevice()->run())
{
totframes++;
application.GetVideoDriver()->beginScene(true, true, SColor(255,140,161,192));
application.DrawAll();
// start mode
if (mphysicalSystem.GetChTime()>3)
{
mphysicalSystem.SetIterLCPmaxItersSpeed(nviters);
mphysicalSystem.SetIterLCPmaxItersStab(npiters);
}
//start average
if (mphysicalSystem.GetChTime()>4.2)
{
class _label_reporter_class : public chrono::ChReportContactCallback
{
public:
virtual bool ReportContactCallback (
const chrono::ChVector<>& pA,
const chrono::ChVector<>& pB,
const chrono::ChMatrix33<>& plane_coord,
const double& distance,
const float& mfriction,
const chrono::ChVector<>& react_forces,
const chrono::ChVector<>& react_torques,
chrono::collision::ChCollisionModel* modA,
chrono::collision::ChCollisionModel* modB)
{
if (distance<maxpen)
maxpen = distance;
return true; // to continue scanning contacts
}
// Data
double maxpen;
};
_label_reporter_class my_label_rep;
my_label_rep.maxpen = 0;
// scan all contacts
mphysicalSystem.GetContactContainer()->ReportAllContacts(&my_label_rep);
averagecnt++;
average += my_label_rep.maxpen;
}
// stop average & loop
if(mphysicalSystem.GetChTime()>4.5)
{
average /= (double)averagecnt;
GetLog() << " avg.err: " << average << "\n";
data_err_pos << average << " ";
break;
}
// integrate
mphysicalSystem.DoStepDynamics(0.01);
application.GetVideoDriver()->endScene();
} // ------- end of simulation loop -----
// remove the root scene node (contains walls & spheres but no camera no lights)
// to reset and prepare for new scene at next for() loop.
parent->remove();
// Now the system should already have no bodies, because of full removal from Irrlicht scene.
// Then, the following clearing should be unuseful...(do it anyway..)
mphysicalSystem.Clear();
}
data_err_pos << "\n";
}
} // end test
getchar();
//
// loop of tests:each time test more spheres!
//
ChStreamOutAsciiFile data_spheres_times("data_spheres_times.dat");
ChStreamOutAsciiFile data_contacts_times("data_contact_times.dat");
ChStreamOutAsciiFile data_ncontacts("data_ncontacts.dat");
ChStreamOutAsciiFile data_timexconstr("data_timexconstr.dat");
for (int nspheres = 1; (nspheres <= 1 && application.GetDevice()->run()); nspheres +=1)
{
ChStreamOutAsciiFile data_timing("data_timing1k.dat");
//
// Create all the rigid bodies!!!!
//
// the root scene node containing all dynamic objects
ISceneNode* parent = application.GetSceneManager()->addEmptySceneNode();
// create spheres and walls, and insert into that scene node
create_some_falling_items(mphysicalSystem, application.GetSceneManager(), application.GetVideoDriver(), parent, SPHERE_NUM,SPHERE_RAD,SPHERE_MASS);
//
// THE SOFT-REAL-TIME CYCLE
//
int mdelay=0;
int lastFPS = -1;
ChTimer<double>mtimer;
ChRealtimeStepTimer m_realtime_timer;
//ChStreamOutAsciiFile data_deltares("data_pNviolation_i80.dat");
int totframes = 0;
while(application.GetDevice()->run())
{
totframes++;
if (totframes == 400) mtimer.start();
// Irrlicht must prepare frame to draw
application.GetVideoDriver()->beginScene(true, true, SColor(255,140,161,192));
// Irrlicht draws all 3D objects and all GUI items
application.DrawAll();
// HERE CHRONO INTEGRATION IS PERFORMED: THE
// TIME OF THE SIMULATION ADVANCES FOR A SINGLE
// STEP:
mphysicalSystem.DoStepDynamics(0.01);
application.GetVideoDriver()->endScene();
if (totframes > 600)
data_timing << mphysicalSystem.GetChTime()-3.0 << " " \
<< mphysicalSystem.GetTimerStep() << " " \
<< mphysicalSystem.GetTimerLcp()+mphysicalSystem.GetTimerCollisionBroad() << " " \
<< mphysicalSystem.GetTimerLcp() << "\n";
/*
//***PLOT*** plot max constr.normal violation
if ((totframes<600)&&(totframes>300))
{
ChLcpIterativeSolver* msolver = (ChLcpIterativeSolver*)mphysicalSystem.GetLcpSolverStab();
if (msolver->GetViolationHistory().size()>1)
data_deltares << msolver->GetViolationHistory()[msolver->GetViolationHistory().size()-1] << "\n";
}
//***PLOT*** plot deltalambda corrections for varying omega
if (totframes==999)
{
ChLcpIterativeSymmSOR* msolver = (ChLcpIterativeSymmSOR*)mphysicalSystem.GetLcpSolverSpeed();
msolver->SetOmega(1.0);
}
if (totframes==1000)
{
GetLog()<< "Saving plot data\n";
ChLcpIterativeSolver* msolver = (ChLcpIterativeSolver*)mphysicalSystem.GetLcpSolverSpeed();
ChStreamOutAsciiFile data_deltalambda("data_deltalambda_om80.dat");
for (int k=0; k< msolver->GetDeltalambdaHistory().size(); k++)
{
data_deltalambda << msolver->GetDeltalambdaHistory()[k] << "\n";
}
}
*/
if ((totframes>200)&&(totframes<=500))
{
//data_timexconstr << mphysicalSystem.GetTimerLcp()/(double)mphysicalSystem.GetNcontacts() << "\n";
//GetLog()<< " avg time/contacts " << mphysicalSystem.GetTimerLcp()/(double)mphysicalSystem.GetNcontacts() << "\n";
//GetLog()<< " ncontacts " << (double)mphysicalSystem.GetNcontacts() << "\n";
}
if(totframes>500) break;
} // ------- end of simulation loop -----
mtimer.stop();
GetLog()<< "Time used for simulating/viewing first 500 steps: " << mtimer() << "\n";
GetLog()<< " num contacts " << mphysicalSystem.GetNcontacts() << "\n";
GetLog()<< " avg time/contacts " << mtimer()/(double)mphysicalSystem.GetNcontacts() << "\n";
data_spheres_times << nspheres << " " << mtimer() << "\n";
data_contacts_times << mphysicalSystem.GetNcontacts() << " " << mtimer() << "\n";
// remove the root scene node (contains walls & spheres but no camera no lights)
// to reset and prepare for new scene at next for() loop.
parent->remove();
// Now the system should already have no bodies, because of full removal from Irrlicht scene.
// Then, the following clearing should be unuseful...(do it anyway..)
mphysicalSystem.Clear();
}
// Remember this at the end of the program, if you started
// with DLL_CreateGlobals();
DLL_DeleteGlobals();
getchar();
return 0;
}
