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);
}
}
}
}
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;
}
